From dfea0a4fb6e79b6ced8b1a5bf5ed275e5326fe10 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Tue, 14 Jan 2020 20:00:41 -0500 Subject: [PATCH 001/121] First commit Created an example command robot. --- 2020-Robot/.gitignore | 161 +++++++++++++++ 2020-Robot/.vscode/launch.json | 21 ++ 2020-Robot/.vscode/settings.json | 15 ++ 2020-Robot/.wpilib/wpilib_preferences.json | 6 + 2020-Robot/build.gradle | 68 +++++++ 2020-Robot/gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 58702 bytes .../gradle/wrapper/gradle-wrapper.properties | 5 + 2020-Robot/gradlew | 183 ++++++++++++++++++ 2020-Robot/gradlew.bat | 100 ++++++++++ 2020-Robot/settings.gradle | 27 +++ 2020-Robot/src/main/deploy/example.txt | 3 + .../src/main/java/frc/robot/Constants.java | 19 ++ 2020-Robot/src/main/java/frc/robot/Main.java | 29 +++ 2020-Robot/src/main/java/frc/robot/Robot.java | 113 +++++++++++ .../main/java/frc/robot/RobotContainer.java | 57 ++++++ .../frc/robot/commands/ExampleCommand.java | 51 +++++ .../robot/subsystems/ExampleSubsystem.java | 24 +++ 2020-Robot/vendordeps/WPILibNewCommands.json | 37 ++++ 18 files changed, 919 insertions(+) create mode 100644 2020-Robot/.gitignore create mode 100644 2020-Robot/.vscode/launch.json create mode 100644 2020-Robot/.vscode/settings.json create mode 100644 2020-Robot/.wpilib/wpilib_preferences.json create mode 100644 2020-Robot/build.gradle create mode 100644 2020-Robot/gradle/wrapper/gradle-wrapper.jar create mode 100644 2020-Robot/gradle/wrapper/gradle-wrapper.properties create mode 100644 2020-Robot/gradlew create mode 100644 2020-Robot/gradlew.bat create mode 100644 2020-Robot/settings.gradle create mode 100644 2020-Robot/src/main/deploy/example.txt create mode 100644 2020-Robot/src/main/java/frc/robot/Constants.java create mode 100644 2020-Robot/src/main/java/frc/robot/Main.java create mode 100644 2020-Robot/src/main/java/frc/robot/Robot.java create mode 100644 2020-Robot/src/main/java/frc/robot/RobotContainer.java create mode 100644 2020-Robot/src/main/java/frc/robot/commands/ExampleCommand.java create mode 100644 2020-Robot/src/main/java/frc/robot/subsystems/ExampleSubsystem.java create mode 100644 2020-Robot/vendordeps/WPILibNewCommands.json diff --git a/2020-Robot/.gitignore b/2020-Robot/.gitignore new file mode 100644 index 0000000..983678a --- /dev/null +++ b/2020-Robot/.gitignore @@ -0,0 +1,161 @@ +# 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/ +imgui.ini + + +# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode diff --git a/2020-Robot/.vscode/launch.json b/2020-Robot/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/2020-Robot/.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/2020-Robot/.vscode/settings.json b/2020-Robot/.vscode/settings.json new file mode 100644 index 0000000..5200b5c --- /dev/null +++ b/2020-Robot/.vscode/settings.json @@ -0,0 +1,15 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true + } +} diff --git a/2020-Robot/.wpilib/wpilib_preferences.json b/2020-Robot/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..8612cf2 --- /dev/null +++ b/2020-Robot/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2020", + "teamNumber": 1519 +} \ No newline at end of file diff --git a/2020-Robot/build.gradle b/2020-Robot/build.gradle new file mode 100644 index 0000000..0e4fe1a --- /dev/null +++ b/2020-Robot/build.gradle @@ -0,0 +1,68 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2020.1.2" +} + +sourceCompatibility = JavaVersion.VERSION_11 +targetCompatibility = JavaVersion.VERSION_11 + +def ROBOT_MAIN_CLASS = "frc.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 = false + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + implementation wpi.deps.wpilib() + nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) + + + implementation wpi.deps.vendor.java() + nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) + + testImplementation 'junit:junit:4.12' + + // Enable simulation gui support. Must check the box in vscode to enable support + // upon debugging + simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) +} + +// 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.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) +} diff --git a/2020-Robot/gradle/wrapper/gradle-wrapper.jar b/2020-Robot/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..cc4fdc293d0e50b0ad9b65c16e7ddd1db2f6025b GIT binary patch literal 58702 zcma&OV~}W3vL#%;<*Hk@ZQHhO+qTVHwr$(CZQFL$+?np4n10i5zVAmKMC6WrGGd+F zD|4@NHj-D$z)bJV;MYNJ&!D%)v-fQ%q0JG$_z5GVUJTPg0MHPf1TvicY#6DXYBBQ4M`$iC~gA;06+%@0HFQPLj-JXogAJ1j+fRqw^4M` zcW^RxAfl%+w9SiS>QwBUTAfuFAjPXc2DHf6*sr+V+jLQj^m@DQgHTPmAb@F z8%GyCfcQkhWWlT31%4$PtV4tV*LI?J#C4orYI~WU(cSR{aEs^ycxY`1>j1po>yDMi zh4W$pMaecV*mCsOsPLxQ#Xc!RXhpXy*p3S2Hl8t}H7x#p5V6G5va4jV;5^S^+>+x&#zzv4!R}wB;)TyU zE_N~}nN>DTG+uZns%_eI=DL1E#<--Sccx30gvMT}^eu`2-u|{qQZ58(rA2aBYE*ZD zm|*12zg*@J$n|tbH%Mp|d|O9W%VT~xG})R=Ld5z<(z%DOO6=MF3Xh-aF%9Hf$?1N9%8Pkev{wun$jZ2 z^i*EhRt8Ve<7`Wyz~iMZDye+XVn}O%qbhV`wHL+%P+n)K&-UMuZw^RRfeQ)%K=k*m zq5l7mf`4K_WkV5B73~MxajljrjGiJqpiV#>0FkyyrB)@HY!;Ln(7JJ*W(>d5#^ubU zVAkTMs*CHzzvUa^nRu0*f-(ek+VZw+@P~}a;;(K=|!9Mhv(~y-mlW);J zb&bB=vySHG`u?j&_6dh^*se*l_B3avjlE|!!Cb0pXyEXRbLy*@WEQ4|)M<`p8Q!rfDJ2RI!u1hPzNjy&)(kcY~GaD6?)7#dCbm`NFh?Y_g$#!+Qrie7%<7P}<-+W@{sxi4JYI{iY zk0(>m$DxOI=~-&eXf2bfh^&(U@o)>(iA1_wJ%B(+nFH+ceib%HEck32QL=J(BNFh`f>St1%llF8chX7#cp*;z}& zcTeXkwsXhf+e;##!FS2yi=2cChcYfzm$wQJ z9%4kAq)wLHf5wfcj!A|xDsAiAOHRzf*)Z-|daN9y5jK-*R{Q0?xaSX-3m|WeuZ`BJ z>eTi@uQ{OGSDIJ#Iu@JPtOy!C?q)g*6SHORg)eAJGh8b-I*X_+xNqZ|OXEsQ-RWte ze`zjjeV9PpE3ac2za+Rs=PA;%QZ>T{x(TRzwWLp_X^2yC-DOEMUy5So!npzL&-@}u z#>uK#&`i&c%J$!bsntEJhY@rF(>6eY;6RoI5Qkn!&<80X5+1(x$T|wR-ad?4N1N^a0)nBj#&EkVvQ?I_+8t*%l#VK&I?uo$ERI1HMu4P2rLMeH%m3 zZ|HA^*O^dA$gb$`Cw;z9?G?m3@nH6TNYJ04Fd-M2wp8@(;vAvJ ztFoni)BLwncQ3@cO*^+6u;(&D<;N;RKb)_NQ_Qu&?@h3MWvo>6FHG%%*smTwj3;dG zQJnT7Wb?4!XmV^>N@ZkA7Jv9kAfD-gCHu2i+!A!}y98SO><8g}t;1JOOxj>#l zM!?y|j5fR3WY2(&_HSGjgMa?Zif<M@d8W z)4>Ptm@zj|xX=bbt$=j}@a_s|xdp6-tRlq6D|xb_;`9oJlkYF1AH%?Pzv$eIAogMi zf(_H*5t({Arfs5XAPj46pjiudQw?dulW-=OUqBVa)OW9E;^R+NDr&LES&m_nmP>Ga zPf)7_&Gn(3v1qu_a^qW9w4#XIEfgiHOQ(LDi=E&(-DcUSfuQE0`ULsRvS}fpS@<)3 z|CbQSi49rU{<4|XU;kiV|C7}Gld$}Yh5YXjg^W$~ovobybuZ^&YwBR^=qP3G=wxhT z?C_5Trbu~95mOoIXUmEOY646_j4ZL)ubCM{qFkl1u*%xs%#18a4!(*b<&edy<8t2w z_zUxWS5fypUp9ue+eswoJSyv*J&=*3;2;q9U?j>n^q?)}c8+}4Ns8oToBJgD;Ug=y zOa0>{VFrLJutjR{PJmm(P9lPzoPi{K!I{l)pGwDy59p-uxHB9I&7zl11lkCu(}*A< zh492AmxsgwEondBpB^{`I*L&Ut40fjM^JS8VdAWQMlwc>_RUM5|Mjes!36DGqW`xs z4tU4`CpOk|vew8!(L}fEvv5&-3#GqZ(#1EZF4ekDQ@y*$tMDEeG?nOUiS-KXG=rAZ zHUDlMo@X&yzo1TdE6b6!s#f{*45V-T3`e2)w5Ra3l>JWf46`v?Y6B&7*1$eS4M(3% z9C~G@N@RXm)8~EXL*9IObA+PwD)`%64fON_8}&pqjrg|2LmP{W^<0@W`9s^*i#F}V;E8~`-}(4@R4kz?t(RjA;y-r%s^=)15%C> zbF;NZET~nybEsmUr8sH^Hgq^xc^n$ZP=GcZ!-X-Go7J4nByj8%?aQ`c{88;p15Kf>|0h+5BLkM&@KI-(flp^npO3MC~W@Uyjv* z6Hu!4#(NtZJ0*;_{8^xcLrC4-zK$BVo7S5V=eg?R8P;BOpK3Xwms+Jt-8R6us zf_rUHFYHn~lu!)U$e$#%UBz7d8YS;mq}xx$T1PIi=4={c-_cY6OVc<=){mOVn>~J$ zW*2PB%*40eE^c+d=PP7J@bqIX_h4u6b6#W|ir<;IlR`#s`Q*_Z8Q?*s_&emuu8D;NSiPX9mK?>$CwcbjhCuv zO&u(0)@}8nZe=Fl*0uMri02oYDjs#g$OHCZ6oTXV2Y0TrZ}+o%{%i)OAJBj2xHC|F5o+`Qmq`$`2EaL=uePwq%k<;6S2n=w%_9vj$8NO|{` zTEg*tK8PU#DnQ#dQ2mMJaaL|HV;BCn?eQ%d0vY@S7Pu@7 zsf5u`T=bL7NfyYO?K^PR_|jap@K|qQ zmO8CK+&O3fzgEnp2|_=^K9ln~QhxjgMM>EQqY@k@@#np@FnZq|C{EyEP7^NurUm0q zW5rKmiy%__KE>YItATyMhE({0%ve10la=mUd<^AcB{T_$Y`2_N-x;F#3xTORXvhPZ7psmqhXy?WxxB5w!m*4&Q;?t$4Kt?m_em-htVDxora24&6~5z$MG(RT{trtp(L( zy&VDT{@p9_DGoq+I|abw$E!TyTO7j6dWQ25dqdKV*z3E?n-p|IG42ZUnNok? zY4K{y{27bUT@#|Zcni!tIgjE`j=-0rl(tVlWEn>5x7BJBkt0iw6j^4n1f2i^6ebo; zt^&Yb##}W0$3xhH&Nz*nANYpO$emARR6-FWX;C?(l7+}<97Ay#!y%BI6^st=LaJ>n zu{ORVJ9%`f*oy85MUf@Fek@T_+ML0-0b$lkEE2y8h%#P^X6+cn)IEXa@T7CQ{fV z-{^wJGN*+T!NsAH@VNM3tWG;%y{pVF2m z2*0+i?o40zSKVq_S18#=0RrJIse+;5cv#a`*`wNs+B%Ln8#e0v^I>7a_33h?lHo14 zg)CbDfGMyH2cj%7C`>|Rrg;U?$&y!z(U10>(dHKQsf9*=z)&@9u@w%y+e@*CnUS|E z*O^cQqM*!sD|e!u(yhXPi$Sl<$daf3sq@Iexafxt3F#2R&=cK z!gT-qto{oVdGUIxC0q`tg)B-Zy(pxGx}&svoA}7p=}jb3jEjQ!v6=afKI!2`&M{#tY$~3LR}#G#U2up2L{} zMGSX>Yjg6-^vWgeX0i;Nb0=gQmYa!|r0rRUshm2+z3AlehjfTqRGnRAmGhHY3`R_@ zPh4GAF@=nkRz;xMO3TPh$)9Iq?Fs5B@~)QIntSyeBy^10!ts?9Z@tK&L6xJd9 zNzaaz6zvrtr&MPQ@UD)njFUtFupwB zv+8%r`c@#asm}cKW^*x0%v_k3faHOnRLt7vzVFlqslue32rt(NNXnkS+fMSM&^u)8 zC`p{on>0pf=1id|vzdTnBLB;v%*ta`o_lzj21u+U-cTRXR%sxE%4k<(bU!orfsJ&v z3FLM2UT_*)BJm1^W;Z{0;z^_e=N&QXSO>rdB`*cp>yGnjHJt$ zcJd~52X&k1b<-`2R{bqLm*E(W{=|-)RTB*i$h4TdV12@beTkR&*iJ==ck*QlFiQ52 zBZ|o_LP06C?Sgs3VJ=oZQU0vK6#}f9gHSs)JB7TU2h~}UVe%unJA!URBgJ# zI~26)lGD4yk~ngKRg;(s4f@PccDZaL{Y=%6UKHl&k|M@Zc4vdx-DX4{belQ);URF? zyxW+|Ziv}%Y!sFdY@YO))Z|f34L(WjN*v#EfZHn6m)X@;TzQ@wIjl4B_TieZY}qY`mG}3VL{w?; z&O>sZ8)YnW+eLuW@rhClOOCZe2YP@4YWKN?P{c~zFUj*U?OayavPUo!r{uqA1<8h! zs0=rKKlwJYk~34F9$q6fQ&jnw_|@cTn{_kA8sUZ#2(Lb@R$NL*u>08yYGx{p6OeX~ zr7!lwGqMSury(v5=1_9%#*MORl2apGf(MQIQTMN35yE3l`^OS7r;SKS6&v-5q}Gw* zNWI*4OKBD&2YbCr8c{ifn~-9w-v+mV49W+k)$jjU@WA+Aok01SA#X$Sspj}*r52!- zNqOS<0%uMUZeSp+*i1TEO$KGKn7EwzW=s?(b5X^@3s5k*80ns2I2|bTHU+bWZ$x;j z`k@>)1G#JgT=F!8awgol?DqK^S4R*g?e}2rOYRVMUKKxSudO(hOLnnL zQqpxPNouLiQFYJs3?7!9f6!-#Pi83{q3-GgOA|{btKup4fYDu-JFOK~Q1c3KD@fdJ z?uABYOkHA^Fc~l0gTAy4geF<-1UqdS=b=UM6Xi30mPhy1-f^aQh9H(jwFl5w*X`Mh z=Ee5C?038GEqSVTd!67bn9*zQg-r8RIH3$$ zf8vWEBbOc`_0U{b)t)Toa~~<7c-K_=G%*iTW^?6mj9{#)@|# zku9R^IDzbzzERz~fpxFrU*it;-Iu&m!CAtM&$)6^2rMyV4 z$+e!$(e)!UY(Sc9n6hkr^n&cvqy8}NfZz+AQc8fU9lNczlP>5D3qzWoR55YvH94^* z-S%SVQ96pK3|Yo`75D&85)xij9Dl8AO8{J*{_yhs-KtsLXUYqwieO(nfrkB@%|OyI>yF+1G?m7>X&djb(HBNNw3KX;Ma*oMV)cV0xzxmIy+5>yz>l_LLH)VyRnYYce zw$?q!hJzX0TlE0+o5QJDM~sPrjVCN7#|32#rUkc>?-eN6Q0RqQTAl~`&isrQg)ass z+x5XapaYh{Dj`+V096?w)w2!Cnmh?x1WmFC$jEFY4;V)XAl3*tBS)V)3TbL)g46_g zCw9pl^!3OCTOcaEP!?==guEAw;VZ}fE6K-;@qD-Rx~td+j(N>)Wv$_mqFTH_wVZNEEuDG!0T`HXLsf+_E=X3lw4`_&d5&YMl%H733ckO){vZm znFLS`;5J#^`5~unet`V#*Y5In3yb|Ax z|A6b^F37!_z$_{6h{7l~<{u7{Fx*A*#zw{GD)6e}n6f<|)&7`S-txiz3Jm4S5hV&8 zm|Ncc{j_~`^pQ*I#w21;(jwi8GnH4efO;R|r4$tH~i;Bcmp^sP9) zjhJne@yzU&XvFNoc~i(wQ?nE`o6Hk~!;x(%xh7?zvigH2g`!v8L-vEN0DvV3?m( zSW(TZ%2AWf`rS}GGMqUj!8yCp#|fR--Vxfj=9}YD97Gocdj=S z0zkF-jsO>EcPTB1zRO$++k^bH%O`=UkHdHT^5?{$)ot<-K2XIE7js*4OjF)BsVjCJ z*KN)!FdM*sh=fB$p8*EzZmGJp?B_=a-90$FI{S$LLjBU$(lxUj;9 zIBszmA*129W+YE;Yy{J~3uyOr<2A(`*cu0IJN#tmUfz2jIWQi_h)_-V6o+5CjbX!1$lz6?QYU za&|O#F%~hmGUhil{M+J|*0<3&{a1%ONp-^!Qx*LOTYY}L!r9BbTxCjHMuUR0E(uH` z!b$*ZMdnB{b2vsb<&P6})+%O=%a8@~$fjbtfF@Z>^Q@enTOJ%VT)Rdc!wX|@iq9i}HaFZAeY6g8xGZY7h-r1sy_<#YU6}I?L zwvf0ePE5PKbK>2RiJOFO5xNhMY+kt`Qi?Oxo&@xH$<^Q;Nb(&rjPBAcv;XtmSY90z z;oIFFl%lDq$o&kYQ;aSHZHD@W({Y1hw<-I>7f_X8wc?%hNDlo~Ig;63RlHNhw~#R3 zA*f5D_Qo`4_ajY4Gr{mLs*(Fxh(U%oua_u3r%`H!TI)@R!!iqV8IOhIOzI@=7QJ=G zV$(9mEVL(7DvPn0j%_cOZN|vvNg8*PHma`6+oS;PDz%iOFyo0n0e%$<#A3r~$=I0T zDL*{AREUGx&C2}?I9cVL`UcPyawTqA4j-4%Mr-4`9#8GX1jiJkKGpHVr1~Rj#zFaZ zqmE!<|1JCi!LDG?1^Ys62xz(p;Uu!QZB7!C0#piy1_9=e?^s@-sd1gs!h$;Q`TNtf z3N4Elsgl#={#U`~&}FNvH78MLjjavl1x*4pNVr338>%sfHu>bxo2#eZN2ee9q#*Jg zDk_=OBR;8t6=pBN0aj)&Nj}pzqqUYW(tfk?bXTdKbNQFSUMCyN-!b0#3?Z;ijzx$M z^Eo6Eq*NO!Y8K;84H4MHj_xwBYc|3>+D(PFj7ejhECG@5@Pk&8dG<)HwwO2~j7KV6 z0$s}=*D;ek#8$a*sxVlC_`qFkM0%BQQ@v2H&Aq@G9XCQt^^x<8w*=MbZV)@aPrrn; z`6r*&f`x&1lp)`5>-|-4%l&W4jy~LydfN;iq?Y8Xx>Sh#2Lx@FXo|5{WKp@y-x;)7 zl;;_Y*-Nu3pcH-)p0(tP~3xO_u~>HpCdEfgyq7V-!ZZ{?`6v_b-vx< zuu|gm5mG6c@D{FYMLuzvG+A2T&6&`n>XM%s`+Qtj)5XdpyFOnz3KLSCOxaCEUl()M z3b~FYqA3FT1#SY{p36h%M^gBQpB2QzEdtM9hMBMRMu{|rf}(;S85&|A!|Aj}?fMKaju!y>_AS}#hRe_!&%8V=6+oPPtE zOOJ-Rcrf>hNq@lG{{@$H?6ikt@!A2OePLe{MBIWSPz7{u(I} z$PXzD;leHG?Xl0FnWt+Wrkrk*|e3P~YVF@N$y&L929cc=#-!*k)HZKDo8!#+t|?9p0z1KSDKclB&M6~hN5<9~^DIltXKR$+iK*h9k$|@Qoy9H}PSI;b(v>w`8(k70@sfa4nRweeiwZ-syP3zPSsyK_8Te9*(FQdm+ z84ZDah4PGehH72w=Q8bx;pK5juT67rJKb|ovD#COI^l6z0eBidn$!Y?T2;5sN+vTV z$`%Edb<%-Oq@NPZy<2Z3m;$}!9JzIuVK6;fJi>>m3q!Lr!2xXRq+l0LvZIR_PNYrP57E#sCvD^4UU2GVr*Rx`QcT}yQanF z3i~!-2Vkk4S%4Hd2baDvrM2g(&1jZaA1!vLi!I#5wX6g^&PE`0-TovM(%wuaPXAno z`a&j{ai=TsgKpc1C3|)tY#!4>SPBbMnchi}glCBwaNE(4`gi}JY0;`|m`s{HtaP@& zHxwCt#2&z9A7O+=v>za}LW~}G>_tWo$dsRX)f1L=+tZF5E&RBA#jUC|N9ZPa_&z5= zekCOsIfOh`p(&S8dnkE~9#(;BAh8qzi5JYT0nP7x&Hga3v`XFdRN|$5Ry#mq*AN$J zV)l~LSq}2d{EJ@%{TLnkRVn*sdM{_b|4!x73|Ux9{%S;FPyhfZ{xg;P2ZmMuA*cMG zipYNeI7{u98`22!_phwRk|lyX#49r%Lq1aZAabxs6MP79J3Kxh0z1E>MzLS6Ee5u+ z@od~O#6yMa;R}eI*a|ZB$ar0BT`%X4+kyxqW4s+D3rV176EAsfS**6-swZ9OIPRZ& zlmIH>ppe;l28`Kd0z(alw^r<%RlDpI6hv)6Gs?GIpffKApgx^)2-6jAzjZE0BtPBC z0z8!#C5AP${zTF$-Z^v%^ie8LI*rvR+*xc=>fa;`SRUSLAio?qL;jVFV1Bw4K>D+i zyEQ}vyG2HTx>W?Ul&MhxUXK7n;yfN)QS`foM!4>4-(PGwxW!^^UyKOz(v+1BejI*& zQSkV|m5=JF4T0k*+|h|3dx`ZKBVX7H4{5iakAxnD#J=9igW@LS;HE_8$lZy1l|$wX zn<8-$u=7&li+^MB(1y~Mz7lj7?oYf%1k{wT#?(Mep094qqnPv7*OYkQ#7$pkU5U24 zzPLEwAb<VIp_uUE~+r5)jt(>>Bg48_{)twH$QJDSBrUS!j{lX z)SK$6dfLWt)c9%Cml+sRp*OHXB?e4hbYZQo!@=6 zBPTpi&6&atD*#Cn6f@5<>79Mq7o0^E!NH)bD26g}?@qg%*AYeE6Tec@F?y9Q8i}^s zz`)l`8>;h75!kL!`&*_hsX1%2)(lWr|7!}@gn%MfwY8vN0=pMm3WesCRv5e*5m4z|u(zbYCpuxO9$bY)hkL|}mRj{3dlRgNK)#PJp#vR=ka^TZ(tKVI<>M~ekIfd2 zm3UDUNW*ZvS5L|SF334|YD>LJk(EqgPpVxtzwclUNaH70zWDVt^1+cz|F?RdF4HHn z@4~Gs`lj!0dWi2n#>7C@B$Qf7|t{1!3mtrO1H7 zi{=I#^Oa1jJiFI!j>PualW+ncHJ)TelW$bv2MqUG1xK7R z%TsQfTn)7D3}XYU+{?Hq!I&fqi4>DmryMiO?!aN!T4fnwq2vsuB^s6fPW@u*h-JwG zNniJFR(RI*?5HV=tqO)lv}CRv_eNEBR%z}Vnftv0+DUH^OCODH#&;{+aw^1vR z-c~|Mk+o?j-^Z+rR4s z-gNA5guTuab7N`{Y@eT&)!xF8#AeetvQ6d!W4BlO;0#0TxS_( zMm-A-u+h7-PjmOQHlh{Hxn+J$jh?uEtc8RG8tu->og@ z86A%eUt+P8E3oLXIrq#K(nCF@L12>=DVT3ec6Vn=B^B;>D=O%op+0BT;T)FHZ`I93 z^5|bpJC_kB92`alM40Am>Yz5o1gxkIGRYQ)x^+R|TCK)r;Qyq6+~S9Uy9nr^nkvc- zxw~#_9eBBJcZNK0yFZxUK4h>u$8;4k-KpNTblRgS(y&u~u&J;O!aqAMYJp+(BED*d z^I#F7vPOEADj}Pziprs=a{%qgz#eso$j`At7pN~bDw%&ba-+4pI}T*?w-z^_~DfD~Z3Tg+#M#u{s&uRF^dr5RFZh7<|WNEG;P z-_SzXTbHc^yD$r;WJqqJkA7^(zN`nzQ5V16nG~Zobuy)a)(T@Ik>V!qOfw;e z)?AZXjzDJg%BkIEY&bm&BczLuWY~k}3Zyx#)jxg1A9R`sz!_dCb!|13b*3PiA@(E6 z9HmG2R>-YrW93UMQO}XE4loI(*er9J*wDUd1se!pzdpoB_v6^lQl}+!6e5MS`+bU#_b*a5Pkt;o+lOV4loyn2P z$3;z-cX>$R{6M4q%b}aMBF}6N+0RCE70bB;XwHV~JLO&!EB)Cgo9ta_>>Os1HNfaY z4PNu7BGhw`6}cm>glh6i^)Ja{rpLHix?C?u;(e&GI{?!E7$9hd*5c^iL?;6Kwn z@qbBE|3UMF|F$Ok>7YY?CeMzMes@CZJQ?&|R8v5M@XvW}jjxhjl`gzl;rvy6Nn9$K z;1TKGpUgZs`vR!t-sD~2ar{58-;2k`H(MIWr_cujtSCpjue(R z(a7R{q`G+;8qD8D1e?1zWv+pPFtk=k#>f`yqZo)3KwCBgABgQbq%hu4q}h+Bdyh?* z#Rlr*$38^Ru%m9FUTQL2Xy^j|f%*4H*{zWFRsMbs6@u{JM{48fq;F;QFV%6Dn!6X0 zEAr2G{RmY8;Jlmws#%7Hl_TvQMbLnN0KGK=9)1u=Vb&#V27UwM#U+)$hn#hlXxBxO zM~<3s(W;fe-0%mVWtZ)oN|h-01@5z=u(z!V>)I9-IepH|_q6NR_DA>2hxGKt-QX;H6(^FXwcBndi1s%qn2sH-rsuON7*ARP6Qt$2XIy3d#cn8sLh&7#USTFn3 zQm-o6-Bnofon2V;oq-v1@Ye@NuH$Z~+th}Cs>F7=H#=4PKLp%-!EwR&0`a}XL=br< zF>&?HNr}9ahB-EA7a({^_6`taBwmB~hJG)p>8r^vq0J_+o`sOq<{s2~2t}W&1f5`l zj;E0nmt?YRp{ONhti9{4&rvt5uoS0CO@%+Yv>+}ROQAGP3VLu^S4fe{ZRoGviEXMF zhM=I=Eg2~^5PIwEq{~Wt?inz13!axZU3knx_)Ey9<)z<=!TnCPHvs1l^spF`@INYQ zY|J1RWri-^D9mVY5Z{u+bXg#}3rUwSXX>&@PN+017W@!L5H8CvZf0wZxQ=UrHJ{Um z$Z;~3t6ARGql*O1^YY(h4awy!h_brE6&k9B&5l;ya>jDyW5?o$q~=1iV!t7#8&QOx6P zhQIm55sij*Ef-G_?k^$AjK2j?=QQ?^=r{MDaGZ7`Yo*Kp1uoZ=&5|O)D#xAHL)n9_l6-E!b zVV@8ny;`XU#X2((4cTmv5unmYzUmJ>Hm+Kvht&a+j3nr!sljTHUZn^0w@L|WKw2TO zRO>T!>jutIzNI5U_KL}vd00oi6$aJqPeJwq)lIr(2Gt#52i@sqCFaWC)pS$pYoRCK zd*$)r6FCClYp+n>gCqVF>x)ghAbl+h${~Mc_sQGk@+sR@b(88l zcx?*Usr}v|kV!RPfS%HK>Bn{7tdEV$CB5Z@=uy4>^(o(%@R|_7dq69s1(X_8szPZ! zSS~$LCX>-}F=io=YcY~9!vqo3&dh9_Mosio`zO6i|$&p;-9%+~sdYNrVE?Q8rS+eHx z4O$l|b3FUT#2jb(WU<`oKAjGQUsoCgE1(c>3byBNPhKeJ7f4S-hBRqRyePY)im;>H z)hyFuFTDqx*ZgXo$hn+u>TGs~=Bjqr3bhPmXG)v8){EU;N*58NKU5;EIZl z9%|JomX+b6M#jS2`B%~!+`EStMD{|y^P=`xPbD$o6;|!((h!+y%7Y{DuC!NCKDIN1 zER-J?vZ$2el4y~!-0vWjNRoC|ARB`IX@M&;?ZpULcAIu`zlH9 z&JK#H);Ij~fqoT{59}OI#ViA%!lPYyd@kHg*hyI;iMdCtw2&eLHOd1*N%2Y!BG*H_ zu@E?VbtZlI{7B{C>A^b3njh=KdF!=rQ!)oIjwkP{t^I{2q&emQ-C1&U&fPC_viACTbT;(A3qRJeGINz^!0N26vQ~o|#pmjp-Zq46%+{X9n zLGKqhLh4`-(*oDHqHU~-45_+pe(BICF$*0jD&FW?ED=vn=t?p9X(%AH9+;6NcJ8JF zASkf}LfT7Z3u*#i$ml`gKIS>3jrTla--x##EDM{w{>Iu9qV!x95ECU*W_O`q>hcCa zswU!;H3R{}(A6aQ(B)lImTF$BzF;$V_?It*+8ZeiZa|b8n_DN4jUfI0jIA6Q6*c0f(uq~DxrNm!$~G=Uz=qP*)?qc(}|7MQZT&B=Um zr{Lj_R7QJAlwD=CoYpjQsUyu1)C9p5CE)%3nb)~WtP;@6(qGG`*qDT zS(zM>&R<;Z23V|80%3s!`0QpTt0Ay;*xLJeE|DP5@x?a!1)`g= z-1}G_LxiiO(*?R*{(yH#&yl|Seyx6*+ETayQtv7Htk3WPvI;U!@h-e$)gw9>pyKmB zk8#$3BF-ou%=`9_3)Q`0ttk$cymvULFS`Khmjes=2(-QY@eVjJ)rSD)z)1No&o+dz zrGItPZ$QuD;Nqt~U{J?9VlM0g{kx!4$?!?=o?um>#7tjMzrLfv<@pI&cp*5H>XPPZ zu8Xh&6y7v0pGDiQqd-~tBjK%-SO8$8kG&44|{09|FO5BoNkV6~JX>g{b#NHJW?gmM# zhbcS|M9fDc44(seG%$hK#va#4YL98mddGDi2qr;@CeiWO!!`DrF<%=_^*3JgoZiSj zdEv30G5`7ex`XP4#6cG;AQ}(|>CcCTGiom^pc*j-Mz1_oGp4iP*>N125YeWCw#L4H z*>u2Ih8jVRJ?rOj-7KbU7KXpYs2UZf)Vf}(lsM(oiB>tgqX2tILJitw_x z&7gq;`b}qrL{lEA3DaXDOi~HQ!^?xxjjVW|#Z+Ek&GKA2dYgO@zB2V*eY zx>@D06X)(FUz3xz99V3v*k7x|wxiFxv>=N$1Chfp>CErJq)gnf=P!u-QKrYnulzdQ zP56u!AH2^QVnuxTJjcQtlflq>PSm4C!$^fv4V_XsIO2d=O8|J`4bUDtjBchJ!14~3 z#mgUPYF*Z?k;Y)Igdx3yQg8L)M=c%}p3!P-0KOuXI+{*LXJ&w)$gzxeTyr`)h-Nc! z`$xa<>T2pbuU0VR?#FPEM44XDRw+cM6U1R2aLQpGHX40=4Er=lp&2aN#P1IA3|r+L z?5jaRyCgN)b(KuS+(x9rPLLjY&4^YY{0T2Ai%`f0p}sG*R!}{DSf7GdPJ=C2MT1ND zUJ@#y06`CNc9n?13R2KY1K*SYeV87wG%bjcIbn+AR8*FS<{?wWomTT5@`}~z3bFAJ zLR-wmE$iwwJ-TnVEhl{{?+??DJ?DWk~VaX-L3-RLtprT2%z-GfD{UVBR~T}zymA0 z6VZ;1Qr%5q#+Oz#3)`D(%WVWWS4BW6%ZvAtt!u25FO@e{X`)_LH>p&pFzx(wvNEO- z!2$Z}`iynmY2j&UCmRNB)9Cn3MXRls&PFVHzkzr;)B^BCMY~6lYY>0rsKT zm4}RV`Q7tbn)Aseay%@-I6ZT~PBsO?D|>kG*%(PGo=|gZ#0zsmE})xxtAvaCe&$1? z(7GyH&^jm!cguuMo@CPA&-lrdE&Aq8GIOuUK9jt{K0ldcvJJp7I`ZMx-EYj$)hl~) zFM!U~HxgO+lb$1cIK-nvz<5OPs(@d4tB6DUa3?-bJ98|dv-kIdtMS;9BuLc{a~_wW zO$u`rNymsAeMH9zh(|w=<*V z&&B{&O0Am`<$iBa)>pNZ6cO`d^3B5%=gmsH(HYZw6!U(c@}#)19F}`BT+yOfamJY$ zYOmy2m^k+ADH2klhAJMLq;6>t3)NREUgk*cjJHg{NBkVhDORNK;v5362&NN=y*Ef- z$vxYTG5Ga{SI&C93^Gsu9G-osqbC9PbsC&@xxGlF?o{!rs9|YpEE?P8ix#yS`7JUy z%ez(_Q%I^RwPrW%rFF(+mE}rp#Wtg@^>O7T(@LFA7j{LNrL=XGDyB-|3<*mqLL_UA zUZz?ulF$5O59-WWZ!d@hRxC@4d6?okW%`1$#<5w9eh>4Cyr#xe5%VPG@TBe#HA^O} z1&q{T_TMTr($f<()ah%TXapiGp}`MAC7>0I=Cx*t+bXy+gMyk*#(A~ft=&4YBdQki zQ}I=c;etc@sD4?l`eYaksPtJnx5OUaZ6u;7p64DUuI`omrWjht5$8+cqb6Hw75WNX z@D(fl7tDl2H)H%QYyX3>cL0*DZPv8+ZgaP7+t_W}wr$(CZQHhO+qUig`^@>y%s1~j z6Y)pXii(P=SQS<4iS=aOnR(rqe#b*BR~GN+bMNQSnhcMHxhVf6D7_zYs}@oo$eK9sZig1_lH0|C z&<1W;8dh6lutS+|02t0VqRfh9R+%!~9YsQ>cw-uGi!YMSo?19?Sty(u{GRqmTx8Zv zLz|nph}CNn+4a~dDzMog(j+NForDvDjLwub!b;p@dLHSBO0kjaI0CPZ)8B2(HNL&A zdr8Pw@u(POF1J*groJ~!1|E(GmnR3L6`P*3C;v?R zDw-pBC=u%}<}P_);mn-_cE}am&b1_WlqnWVzFS;*NhwoOb%+#0nI|H*Bw6_0R(=Kj z;7@eEqYkW2OvWkoz|yY1gZAJw8=>KShthS*ANzYdDT61^AK)>0H%LV4q3}hw?bkA$ zF$tz;<5T59v0Zd$)unmJ{vu_7eGDP6+pe(H&n^3E)g^rB?pn?GT9l1gztAUpR*+Kvt=FE~M zq5rZM&9v>ww1mzrK)vx*0;;?tnqA@Q;FBC@$2~=gy#jW$bAJUNIl_YpT)``*9nnkV zF!&XBK8(PeQfnScH*JaYqy{1bN4MwF=&g2)`!Kuo165*d^1Sc_d{I4>6V=>74c%g4 zXE_M`b@syq%jQx9VRp@ba!rY|MRhr!S3bN!1RT}^I(2gXE`KT57Y;maGA&dHM#`4* zy%@6YB0A6Z^?fg!$4Gq0auM47(jE$Y4osH zhydBwQ-S~vMS7)hg;AC=MRf~AHZu|Ue*bk=ff`!Ol1%=|W-a+~l)QH04q^oeMZHj~ z8$8jQn(n1#O!_7sg1hi;{v%?nd&gK7tfN3I{A0j zcg`ISk^Ir4G=(SvV$v}DE(nE+%rgFkT%cu5VR0Qa^H4-xPC*7Y*+E8#xvyepS#xYE+FyIIi0|5$J%mKAB58%MgleT%Zx42e^L`TdA~Ips z=NvgHNpYZju?*J>oNcmd^(nFUc+-bu4*+9)qIwU^g?1_4-&-`uZm&f7F^1?@3IvJc{gnlh?no$E9jFIfJ8i+33;o-!b2hD@}}{o}J4{l{44v z3Cd{3Lj%9^E43SBXmIvwsA2_8sXgRu=4=H{j9R(fYcCzOXriTZ51l+HcXr@)^?rK* zmc89=w8MW+txdobBh`X4rMvY#vuv0GIEO67sgL}mIw$pNW6s8Fd=t z@58{pFs^Oz&g}CPr8EL~QyUjk&}1qyO4;-6m0MRd4J9T2r5_j+YdeKP%Q+jnWNdV| zUJLU&d%m|g&3B83R^8K^WM{0at+=9UdVAzTnL+CqdcT#($38|-fQ|BJbHY4vk=ANj zvX?ek_oYp6t8bQz-T){|-5OGrv`IGd?>X*h(s{MvQ{j>fZbx<^-)&(j8(N+z^sftB z;V$0+Wd0oUR^&)Q+2bHfLt#V~jZT$UPUbkd#vD#zZJ&huG+-;T%sU~ONA?a`Va|T%I0yd%0*Xr3>p#slVg7Y<6o&Bx856S zg;7Q>mCFF?xq_m}VG5`(0fIX(V=yvQ;xjpwNhrLFMui8xdBw2aFOvI3t6-NG3%+d= z>1un%A{1+tFrn2nu2%`-hiqYhXDga3%{ZVkC@ROtTcA;g*E@K4i_G1&^P#Pl_9*m& zwBVKqZhrf4bhw@M)78cm zBMB!;A)H{6h6AjEv&|DGxYRmY|e_ARf_dMIvm*-i4hR#IU_#A_QYP@L|sHs zo@Ky_Bx6e2??_k;7vjibD#pM*T7`h9V&s(moOn_x^N|9{gkOtFY~gDqSo+7meUjBR zK2jiOsA%PwD|1*KC^m(-WZ5j2AWi;81kCi5t)KouHKt|R6m{m!!n|4YN3yyBo0mSZ zN^yj9>I9Y6dI&$!T7&$%3Ccxua0-&DoNJFbCV%1;h^-U&1Q+@47qrKld+QNGOrh{a z27PfD|L06XuL1+ZMc{_7rB7bd&WD%*lbypj>|K|<#2#t+qPXH zTm`5QC)ktLW5+G&4lhvX8DgOK)|mvQ_b^HuJ&=wP%Z6%;E+Bx|#|Q}vOoGR(jK}sD zk9x4A-V%Hs#G>J5XldT-W&|Kv(!mEi;J38jdK>L|Q7~<_no&|~Fdc~yhC~%VqQc2e z2|pva(YaxgaE`xa5=u=WkhtI|f`XRHhA6|>1`)hDgYzt9kByS$l*OQ2O-a#Iq%SLz zV^&-mn{^KrM6&BueyiV}>&)9rr)de2+DkV8##PSmko(<`nqPVr^n_V~UoIi`_yVdB zzcj4`b5QijKNrR%0AYi<`{NDb!y1^#Pv|K2N8<&wlO7-JDa5Yp?eM)pf>PbMq@)Wr zvki0Y1yLr2WfDb`RBPgq^VC(KH;ofR#9^i$TaMi9J6p5TP5F8<&ofnvL|`*(;urRO z?0k?7WiOd&^v);ux~R9Hznc3moOxE+O$lYV0Ku|hENFV~?Lt!QZlMNp1%d#^Rv!pC zfq`*V)n<`Io8N2XGBOjLYB}#{g#>o-?Hmb6$VyvSN@nI?3{y-pdNvcYe%&%CIeh?s zWfdM@$o~R)P|M>ElHW0BAMI=ozdH-Fle#Dvq-bpmPg-!rDY|1*o|1dvDh9{`{gt%n zFemDyrWMrywXJ+rV5r%UR~0T*75`i&rM4=%7}ulJyHu{rZw;C$r+nn@cLyLgh0d-A z(3SS5tW>ZK0in8bOH$vW>HIcipgUXYGUq49#>Ixff27cCfWz$0vR4Dmq}CBw<~4Sh zDe9adM$vVItE_)3FJT5Bgk}V=1g+Qvf5+hpxwh78gHe$<|r1^Nh?B&_~xSq+nVdY+~dc4GJ?e5EpV zXs-H~6poV`Kh5kok2qSUMD?0&WXKs7T0?Z-J8zti^WD-*_fo zhAqM(p+l2*(|b>aZC+?aK~^_VCZkP0>}TxdEC-KcmAx*YS?wTK?cW>PjS+NxM==Wg zg}e_*NcH%2(J=+WVL+;P)kz0c@48^4ZuemowCO=rriJFSD|#7D2oO{}$kCbL0#0%2 zQe&D2wwJ3%d|+L`bE=&9k_~(BOe$ZFap$YMGL$&$D0=mJ9n%He#RRlC3f=|WyrI0L zA_qS=kzzw8f_QiJYg_b?xA6UgBS0tT_Y$!9>(J-Q|m=O+8+wIPlb5i=-aU~kBf=4dD zd6Q8*EoKqRCcMNO5q%nez-osz1XT6PZ+r7r7A_{!vpDIfE$$yCUU66H>HOUO>u7aE zs*>|KS24COy<^3O^xXssCI`2iF%;A&7{j1UDk9dvv< zsUbj2HMoFr%{j!bRrmyt%jM|4UKza#}%Vf*_fEvi$*6J-h}oRdsdinr_W1-)p24zB*p9tfDdUa27+yi5W`#8+~eE_NyvNZgCP48jF8P; zgYS#IP!@sLe^SeCy4jwre}sC*A4Vk3|EzFISR4QEai+j{bL%-B#Nlt4WJN3eh+Uo) zVtaBF&A%PtbaaH`A~$h0I(5#|WARn>4Hbxy+Jn-$LdJWL+&({?oGdxCC?@gw`D44O zZ)fV$Yi@4u-zGU|!cfh6Eq?2C3Nn%TL2ZoA1+5g5O#q6$QGS|1C!;H{)PU?dDlSGU zLGKxOa;zm!C-Zghet4U7l(%LaEQnKF+>ECNt@`F07q-JO?%%X~*k}Yndc#f*iq0`hgW#iOvymYI0Ur}T;8qZ+%f1paM#v7e! zUS~+CMQqEbYZ%Ix+4iKAGa>>DLya7d_5zQo_zm&bP6F_75Qk^L7A%?p74r#_+3V6R z@m)%h$SZlQi)PpLLYyya^FulLkrPuM%+!YnWBCX|f#M*ph-`6S5IH3F;Os;ZZ&cDq z<~WF?be7SQre3OHq63A%t27ee4>e--Q*N)lFkAI_P@Yoq?Bd0s)IIqLY)xtXU`k>x zfQK0;b2n0v{oPhQju4$`uD>)Syw=X_l}YEfVF8)awhULL-sJNdq;z8~(wyAEW&sDx zxqHk8ufaTXHNnIUP~eE&k>D!g#IVt73wHY+ugJwtuy74u* z1qC32jRV4EWbz*0B5d5qGm7FB;V0Z>C63g4n6hW?!BfHU=hqZbuGx&ccdij#|lWok>4#{m^Fy>{`JdOS zjIM(Tuf4sYrJltP%2vW!U)Mt5hd5_vs^{onYW=T{?nF6taSUF>uPLMY@>8Y#vd&fU zJg$MqI>EOkIj}Gpu%?+k{%zvX7zqvMeuMm%YD6eLoHxL?e6eW>J~|~Z&lHB^r_Ag0 z{*SlMeG(r}i;4UY6e1TDhAnY@tyh=*e7>7?vlwq>&py69o*=hIE389P!iE)Fe1v;HN5fVGS&&jBzQk*Q}Rb%{FF5H zt;vL@*J)TU^_AGy%>+&9)+R@9XQHe9%Cr#w>Q$NM0~WAiktZl>9`I-Ypc0UjVU1rn z_FPNg@88w2iz;NHBJ8)vM$%1oe7QzSs;NxSieG5h->Cq6`M#YqU;tx=1hYym@h%fi zzWLOcEgsbZ>jW|mkR)qpxv-Z}J6iTzy?L3sZiv!nbZ3a;A~Hu3j6-^%FcrouBW^*9 zwOO;eD$2J8edza=ZDF&}5X#=B9O(;A4zyM&5yTvxuoqjP+FZY!ZYI`_D=;czTJF-e z1-$=(BE%9~*+c%p5UT&+n27&>tc8D77L`o(F_e)w^~KRuv4^AdNE-D~2I(p(SCPRP zc{V^gm}JdYd(~~{max0nhdPp5j3){eJ z$LuzR9V>9)451K&?27Aps3vsd_bU(1EDOA~g;@vOO2Ty`4MFO9u=`!_wEKPQp>9L& zzuUbCBGHhsuxYBy-^Uw`)=n5pSF5)!a6qfH$^u&=0GA(}B-Ixjj|ce?Bp(~$q^7BqWU|H8 zKU!?5P@+8*_63=^7)|h<=`vW)2%PZF(`Q0Lr0x5QLjWKIQZB9)OOB_ISy!Mx`E{lJ z1=1d&Ic*{{_h#6sNH^Hz)~vB7gCTbuUkVrOm(pCye57-0NUsKiFMeA#@NBB+F5<+s{(H7mQAPQx`OR z8xRz&uf&f&-?8paW&Q%EHCq$Lv~}lCIW%s>Wxj&$Majn9D~*{Yn8jBZ3b9-fuz!82Hn?&ZI2_JZYAy$kb_?7m*?J z7EcrbL2*)gJ(Wl`yg~c)vC1w>dR$LezB90-T0%EZo|KuQOirNpKJAd) zr+w2F#9m@j64vevMEx_$M}ESx!oajKsI7|Q#c-fWRsS7nAgMlxf$l`eoBx6_u1LP` z5wVEEAYNPN*iXKJza7=aP+z_r$z;5})SQGWl0SrU7qL5T>MpzjZPVq~an6pv29s{gIn1Rh z$*Vp>0p=05JN|HRiyOCbpgpZ@;9Xj|o3DNV!%Xn6t3hE>(=2$dFuEx{osGXYv`m73 z@j>86*-gsSS^3mR)HB6Bj1fy+E{@9e{bcRLU_iAqDzdQUqG)+sqNE`h1 z$3w4loJ+!{F4NdK!E7Vu6L}j5d=VnffP!j5b(b5(u}{;?o9PB`YLsrEsOeE8IUM8F zj!}~kYF^$l^i7CS$AnS+a4#EnWySE!?hNnzWe>=ETyc4WCXpNzZ9R&vLWR9n2)aFS zeT`FE>ZzLpjPr*qdk%A3<`U8cpr3K~?abpqM})l-j}Hz+9tJcw;_-BzCtzpYoNVk^ zd4xI@9~_|+Y_6S*Kx+?A$c)OqC718Wiat0Sl%qFMhix0?j{gw1XO9$zQhjjoeDj|S z8hS*$R7Ol=9=Sd-9s*OgZAC1sMC*(iexn}3CMYJdNZu8^S5)5@Bxo7ayS4fG2D@ns z(Y9t_4DB(20CAx~=eL=RM?RRc4|4V{?Qe z=>g3K7H^2nxwHm|*N+zhk9ET-=0ak5wZAxM<)DFY7|^q+@a_=>AXMj@vZG11mH%nQ zn9XfRt7)!V&u0~v+`DaED;5~WX_cQ6~@iQ$)`#bKdk&+uvYtZMGQ??&zRmpw zbc5donS&q;jPQE_7rh5{ONJKBM;cxKH>r!f)K=VDf}bfc1B4Nv3C}__D{B|kU4Q04E((6!W^q+&Xb=m`c#S!$wEEp4py_0 zDJO?v%A16hzF;#-Lt+DUyec?VXUS?%21=wBiJ<}TTQMa&n$+5wnHr4sni_Hb`tFO; z((Kg?Xh0p)JZnUc=-mE(Ls`z5)+Qr8;F0R92sj9yEJx1kK&wQ8S2S`)h+Qk?^jShBw0n z^g^Pht7xCZvs&|5W95{bypf4acXhX`O_>*QyEk183j48^Ws>JcasVrhs5G9;&2dyi z%>jCf;J1W^x5i(=Cvt|^PAWSdNG}XTJ@;UD+R!_#xn5!VD8@`C$I>Ipes@q*x>0`l z)z8=i*VF~+bxTYjaCr)lzaDau^|9V&q!IlGwQu0TKbn4oBljDL$D`d(xUR1D_M2H5 z_D)E{)YMOgPe9j&Ta=X`w!K8L8Fz1tOon!uWan9)huounS4Mh4dF)BRXPW~rZ){=b z8GKrX8h<5U_7;gkNu2?Vha=mHR?g_-tDJ7e(~;kBqw^DncZb0-heR1$Eu84i7(X`&aR*AQIwovW z>fz)N@L0uBeI%!;>fF*(y?aB?LspSl*h;#V3|hH@lSBCC>z%=##r4vBD?~% zIcaMD#Ep&MMR|QloYSVm4m`6&D~o=K)KUR!2dn`e7}AFYi4ni=M| zwlXp`cKoTc{O?pVGTu@effshzIQL;~Uran3$O8b$6lS*o0sT!BoyZd(zz&P7axA%@Nz)_qI zkD$LWxQoOtM=CJA^aux0eMxT|$TTV{XcUf%R6YWWWpb~~Wr+7tk~!$o(-O!M!{#H? z)jCw2taNz0WO)=*Gud3!7Hi9?DqB;9JQ_pLDASj_PC!c^M|om%q>Zz+S3oK5Y^V&l+!?6vHO@6@c? z%)vqVE`pRD|ItbFC1kt4ApdNC)&9im8NW=RUr>

@up^y4&I8N>~wvL%f(S2W%NN zf&x46sN${5Gh+I9cd>g-O|x3@x#@hdvU54zx*WtnC#5%quWk43w{;_G!4&;N;wy-O z?urjbDnKfp2u4gknf&*wBJS`YfdzBa#pf^Lo9ei}Z)MCk6MP}h0OYrd8`jVipqsRTq}lh>h#|o4yiA zbPQLKXatZ+L=I$?XEGfd7x*_lf|=3xKLi)yj}jQ9pD+OPrv;Mqe+~uywe$sD4D}uV z4@_J6*&E>)?K_L=^f9)ZpbIb0tyI>qF^OuZ;8LrA_T9JRowWUXNjyBVFxj7 zcFv)I!ZI!9%3&ro1=#}qZ!W@`!*%Do@xlC)>lS-KJPYY3@3mXj^ZUgyXXo8DiZ)0M z@ORv8NQ5xIiv%yy7WuvM3l7ZnaX8M-u4s`LZ2-*e2V%BIin4U@4b=3ps|#~L^v#DXv3GDk8H#;lK%qAV<%I5Z8dd3-sIMfqq2WY52;$Y7| zC@8Z_G%EJ3tOhCq_Ad3l4=IN9=Ee$7k#R%^@JPd7SnqL~*a3EWdfPj^Ft)B}bgnkr zBT1I)!g2ha@JU#wQW1op@1SkuaGVJcEJVhstebVvoHV+n`EI?;^p~M~tfk#K1CBi- zF<+3FQvDXkoVE)E6Bj9T)Vlo9rjgCj>S}EH&DnJgn49L@7ZaI=v&F?OY*>NLOQ-u43cR-0P{LGZCyKsW{^hNC8iDiqJ{~) zNqU!S?7Gb=jXSc_T>xTosLbq!#)VKVs^hKlReb|!_v(O0B(=A8tA0Fic+K)>Lc!(J zge-eb*cuWjJCE_q)D}kLQ`X73XAD=didg`EDAk|uw*rjJ1Yj*bj<;`v&pOnps=(g<^CaeJRd*q!NQ`O zTAcA*KCphxtD>M<0l)OpWo@|W=Vs)XFpM7C;96VQR+W3~AXoqC9@yN@7J9kuboR-H zHL8|U?V*D#Jg&`hR95a1#ByH}mfw|kcIP#b2%C}r_nxhIoWdo%k*DB;N)%#~P458H zR&1-?mh?}HxGi(-dh@nkK_H45IB{y)%qwup^p85vZeUpqh|G;9wr%q$_*4*|PS(bw z3$<2M;y;*(WAtHSM--PRyA1<)1Xe^(yuRRaZX9nR0oP5%Wg)P(ak|_q$^7Cd)NP#f zFt*;;hP)je2EkvO_Juc*@6Fd}(xbH@+`c?h1(9yjJzcLY^!{hs3;2?q^IfrF`+D{7 zeAjrrb~tUbxms|met4=I%jCVN6O3DEeY8_%NiNb1EvTu>AI1J!n@36jd$2##c}B>0 z4L;|^v$`6=K#^tk;MTA+ji{smQT)gaODj-((|WI%X2JbpJ46#0RZ&FMJeh+Z<&>04 z)cI;7Dm)CZ1Q9H0Ge@zDXKAsB9dZbg4?1joh3}_)K2k;c^(s6)kl-$}hLll_T0$(y z-4SgpruNv#}%R(l@3!%tj5l!d~Np>{BXo}gF5QWAP7*n?JW-N~>|I~-Sokci&_Ho87f;meu+(2@Yz45X{^W92m`3_^%9FadE5^cGO72ffn`$&G} zGOIPIF?FsLh^0eater8)<@~LjNIyP(W7F~ackhd7ase+Gfo@-RBG6$Q+CeDbE-eiO! z66k;0^Ze3P9kEj(yiZ!_vx)K5>+Jrl2af_iKMbiG*Z6y})9{?`w@LyvBpEEC99HEm z94J&4%248p>c%Nb+Y?Mm9%w8P;5(?F8nINf&_*-><^LeQ6{hj_UPeUhLmtxd+Vmgt zX+WF*G|x;d1!gF0D5?$*b6|tDV#m<_?(f{b+Jd?J92?)y8t>gZ+-KQ+Bj*PJW__xR zdf03Su)GBsi{L~F7m?zTiiu`Wk!YO=QO{H#)PP2?loJ6bfRs0oKxO3+aYm9`#}5V$ z`x646$5C08JvW-c>mV&jy+a+V^zH9IQ#Inj?BmB?I0~jhx7qLD!cSQ9{<) zCB(xvh>|7z&?P1A6fTeZ=vH4`HaRJenyQMrBMl$uNuOX#!uWTr0YsU$pvq9H4wY>t zl^X-E=|ppy073iT6Xv?zU&~*SOz)S{s$uTKR(W@_aAsUm!9UD9D`~`uK!3`Buc{%2B4{J%ioRlMx&#kB{e!Avb zJrlj#<)~p=4r6CfO9_3Cn1xhg=x7nk+LY}yn%fvBEBY;q4p`CSxj7WfX^CU5+@tJWJi(W&KcO*jj5x;xDLZ*AxFvIAYA@P8yW`o)9#pos(U zSgS*I-N9vd=^11lccI*yNQxzMgJ!_I?64MNHZL9-U_DIfm>8g{k^fj)WeFHM8I_z& zZ3l@3<|n0jQSo~R0*Qcqvf~?+vNohOl*bzy=)XeN;2a3p1~0V$$gAWoVuI=*iPkyO z;E~luur&+0{@(mshrT+g9pcf!^T48w$vch$Nigsv6ylw&q=E-ICa#nDgi$8vmBC($ z=yLuLM0U-^2^S`{_ZwTz$|kB|ZzUr`AM@J;{X1nZJEj`$4skl+fss?6#-GZt`JdU# zvVUW}%8!tF0rBe>`+r}#|FsnVkBs^MUX+ze>dHSpWnWVCqdl~T@Zci3NHq%q1q0&Z zjiRz*rIA75MSd&j>=Hq=uts|mK)cc}S884FYT9`Ym2Gbq-?zNU&7M-!u<)j1^s21K z7oJaB$L#M;cjw#E-oI~{yJTr2o((;6binRCTJm*%J0nrPf%?1jgigQI5bI~2dsFN451~NyCYYvfVfu5!YwE`!Uv%`& zB-2spw{|p}vcNP<;@k3}sV|3_r|H|Z4JC9~&KtI*)@JhM?U=mg#m3PjRVoE+M zVYM5uWSO==K5bE81EEz2?F$jdRB^ec45FWK&Dz+e}E=Op=h#{z^;qey2Dx+2Q2qzwA-MpAB% z6U&685w0+}tjouEmcVXOF$U)7w=8u*B7piVzASTr-X|xfrQR1uvc@IZr$CD4MUVF| zMre!R*v|cBT}rB>9#r~c4@(}lBCp$9)X`O$7f_9s)8|{>$Da!Go_qr=;4rtnr7TgXUpffMV9akHEvEw*Z&g!2Env6(!b;)$Zkq!j9UGy>Zopi zUQ<$5Ex<;BxM?&1+E#8>B$er2c?TqH!q^=LX)1lV=@=!xtMbm`$gt70@|} z8AM$V_n1o@=*E15EncO@{DFc)hEBSA@Nbk=GkNsF#}_mBtmF20k$-)eOP+G`q*EAP^>>5d@ea zg6^gb37{ol+=uYC3->5=jbqd}&J|19Oh}yYviQ}E@&>94`r85c>mo=XKA{q~2C*8q z1(8IqD#!fuWdW8DT^RfX)ssdyOzHq^sC=mmY``qcE8^g-o852h1`FBL)_0fHqqzW%Y(brO+X5H!1sl*7|2>*^XZQ^Um1qp- zj{+=uY~SxwTj1)2rmt7luK=kSptJDqqF#W3sech+R{=RBs5U1mcd@_EU~~8?dsmUjsf7tKBg%yZYVwFEDFu zWWQwnb~$%v)IaYXT;h~afPZz{4^@br zn($GS68Obz0BZLqKb0MyvEEp-F z%XZOu9nt29ll>hIY!o7Ulpi znv6Q&d-;x1Q#smNV37IAjmqJ`f>4;j)zs}@5Ggb8NHQ&r9}YcFk1=s0qSmfDIT zL}IzQfY+Hb7z3YWw>3^;vPtIw+@lL;+6f0j=R`K1?Rs$3&Ft1)@NM5zV1L&`Vbl&7 zswRx&Edg?U7fqYMBpWQ6jO&vI*KI5odc0(9&B?LUS$lNhs$&T-QLab-p|8suK`a9N zU;>Q)dneC-M2!FT|4RScQqNRUcScY|-Hb2FWK7ixX)w*zIKVgM!)R>CsoYSb9@Lsy zLJk9)H;@1=N~KM;fxCA80PT1w>bSwB_El6JKa7XzdPVs_qfTy_HegHLC>RgUxX-lj zs_$O^k~(_!_WADl_zRBtc0-mj? zs$_XlVRk8UA;TzI%p`NZo^_F0EiGU(u~@&bF!!jgly!a1es#9LBez7Usio}j;#J*M zYwchj{qF*wFL`?T^AP-=5n(>kT+$T_0iGHp4PM3Z+@Rs&k(ghDz;|7e>IBW%Q&>Q* z*|!8m`k0#8(2SfZzjS1JdAS)iL*a3Q>Tt-uHB0^>6;1Ac&)lXvA#A+^~TF&^<-Px{Arzw?$8;b z6(xcC)ary#!{#M(-LV!}WvwJ94Y}p+dl+)^9$xeZPD9+g#b-y4E)=6{dZvMSy(4bs zQqd@m1o^6YxMp0{hxGGmxj9Cv;|d+QcXE|*vQbI!0Pil2SOuAXlwDZl!rN-01kujv z`f06S5M~gsjn6G_ql(Z9v;Hz>hvm)t+G*Reo}Oz2DoZC~IJYFxV3=*1bcDI#V-ehb z`yS4?O;M_uUKUWRm9-0*%jA%+L}L(ouJ)NW*6>k4H0cLNq(fNgHv4Jnoecj0zTR!} zd#20Z0rVivt#5;(=aRdjZc}W37m&` zO8hf+O$5W$AK*8A8`$z*=vRHy=*QmoFlAg=(s#RhNTHVYC1}1K@hC|GVLZ=F6-*0x z{+sO$vPen^=y*Dt6A!PzJ!}(6LIqT()R5jys9m(YH-ka(Nn?~~Rtl-H*pP{zU-MQ? zlXus*&2qLymA^@KO>Y@ZjhbR)e1(|kVQ~2STn}zH$Hv*3wWt5KBjg$eN#@{G$fcMS8-`5K^IA7m_aM6 z`$)$n`bVh3x<&!)d?X1WLQ9uG9!?;qPGiS*BaH;RE}RifZm9eNEHWtim)l0DD^SyZww8iac z7r6e^#bzT+IQYWSF&Kq!LAalh*r_;Wzi*>jtu~LuXq%d^sr49_?y34lr!u2w+EXxL ztvGKYoa^y*IC%Ypz%YnJV8{reNW^fpBHc9m`O*l>0iqm+au0Ze=X^~VrnQF?&PU+5 zvDnPzI3)KOpigkw6k+Ys(1~ggta{l}hmoJQoMZf-VJ+IOf#vtk(!25;+d@FGwm{aR zAx2bT?D_&PU}I*Rt}$?_UtrnE;npz+3Wm#cQDminaPZX-ZsD&rZgNMlOP>~lPs)5- z1VY9g@uu8tU)@>Vy33Lo9Nkp)j+fdu6g^!Frwn87+^Rz~KEqIZNvGPU)wR*jLB$B}I$TO*f~!7t4654oLO6t8V2r?1+T_Q&0K0 z4682u*_{u6j(?P@{;`Y5=-T~Y%Kr<77Z}0&gZ+aQ{5EN9gm5}+3o-ZC$|VI0^CJnl zlu@4piaXoYaQOv8RMg_I3w0k1bN&6lEJ=n~1W@$^LZ*+5?6;J{!0RU%BNqm{<~-t- zYBiVcsKMtWrxI-wsbMy>B;oLhCnBi?O$~EZ4$9!UcL&30S4}6G<>y$P0t(I%#Lna} zX_$_w@IIB}3veH9GP|^0P;_>@eR7vav@g)kd8j3{^_~v_K#JRObGNy!PKV z%zyngxUd z^s@D@xs>D?9|0^XQSe9+5fMBr9-1rL2ipylxZmKI{+KWoVU3B__h9-y+tCNq0iyqW8C?N<_=wTWv36hc-;u6_5$-8<-iG^wVX{rs#%*o<0 zP`zZD%9FKz8kA)Pi`QrR2c(!`3^|x4*s*D2BB*E3p1pCB6wSJ(K~r=?GY2zKWbkSM zk97>~}>cv zb$Jz&BN$J`J1%`SPSlD!*ydwZh|}u@DspA$4$sz zuve=&^SCLUwSd_bGS|G?7q|}mlM8;PN?3s*Qn`LoL_I|_0v+g4G5lm(&>D&~sR6?l znI)Ws=bL^}57Jk}tm&JypgNPrn=57ljDoPx5vC%_rIdlHBI-9tCQd3ccs7 z8t-*ywH72aUrR7)OSDPqV2JeQ%}`Fj)8^<7+S({A|0d~}AU_#mFK*xIuPXctHbR_6 z0>4#tdv;L;zy3>@ngEyuC~{UEld$Xby%R!P6GeG0aQ`p@>*JR7p_5+YHPKN^V4fk3 zP=|o0bY4goP@xf7HieU5*Pudrp}QZK@B~{n6cMl7DMdWz@t^;~@D^eU<>!6(45Z(_ zk$+hp^uOOo|9MRR!MG0pHBKn;ANR0%BC@7!gZmJPZJXt>$m&mX8a!}cI&=T z^1$X1PVvlD`DVXD#eo%T9Hq`v^hcCB+%v=fj3To3%ZWn%=JZC_ zoex%j4J+ zbQX)n1VtYQf2U6; zl+lO7)ctA65@v(JWy3f!Jhj+syx9tcQ)P2qi3?*W-Zw#Ork|#Fs{k`fVV_!Mn!xL3 zIk}JIQwGd7Ve?#cLD_l3;B&IP`k1Ad;eT4RS=pW5A1i9B3J!lo3 z!WN4Denb)1o>9tu9*MQeIgR3$ z0rD%TiSRC-!526-Q_<1bGYn58#9j%95VT-muFHVK2w+EN#G8i;i`sA@UJgGpB~}7x zXT$xV`dKsMX!X;9Ku-Kvd`_&(SCYV;p<-2TVNbPS!mBJ-Wd&_+BDCO7!-ztt23Z4X=cs@kswD@}xU^1g^h~pu=^6pW ze8CszeDle6mmn7p6^EWdfD|dyNB$Hf%@?7eA4}|ajD2dyBKnD5ou30#)271<>qDF}GnvD)t$ z2fj&M*=&%VGF>YIAwtb!y?Ie|YWR?x(XuT5a+5#3i=W?qc_A~KjWxnJccu=Xz$PiiuHzL7#&Jt#VEx6v~-8J%V@+^q|MYi z{c+eNd4k(vCCT3b1G%D0UknFNZ?%lsqRm{_Bk#15n|;|H)9O&HOroVE-FG(hc4&ZE z(2P$V`Y^c7#KE)tx3Id<0tT%cp7~`AFs#cqf_JH!mS_Fm3^W1T!JXma96S=IrQy{} zb0%%7OB-G)J8g)5WpUWTd10Kg^gMRt${vh%)nB};`vmNAbL>TCRA6}wIE<1qWykbg zPcCUTMV-!d>owCDM3^BD{hCpJcQE*pH$gV#ErC;Wx|Pm9SnipSi4GEzX%cltZ8sf0 z4GJEGTyuxoh}YL_^g{rSCj(Mn9xB&ZpEqiyz-a5H?)=3b8E8s zNV4xhy4dT&cqJb_1$w&<_Ly*)afAyxX!#R8gU)gG)(#SXrbXZnoP4uq5;X(XFv+a6 zX>3lBn@9^3=&!a@Iy7C*kVuccxvO@qV6GM z%IEWSgV;mL3SA>lp*KOzvB5IVgDpwgX_;?gI5YK6==zNjtGgy=}3pI7Ml z*K=k&-d*&zJ{n?u+*PW8qBhLLy>UlMZiEIK|oHw$2rs9WFwD^(_d8L4@aT5=s?a8c%PT*VUVg&tO4QDy2SY zjm2bF%vg0dwTFqL)$eqaDox6HxHo5b zNFgp5r*h$E+lpT*h%KuH+&3V2#-tv2SyzkL$JGiwZeF>fbV(hQ2BwSr_!rt3?1T{# z3+p)Tl>z*Z!>MQQ>u0C#>Grq9WuFghUm2<38IZ<^qz{5X#CQaF zf*+9#(YJ9s#v$mL$-q)RasrGY`j8?J&3!QZLlA<|;QEREfPSG;1T6Zobq2^_0kt5q z09VRDG;Z8JCf6j{ENFc;@3BBW=)L0zw=Nv`9rTWlU%SG*pCtHSWjNhK_eeShOUWc1 zguBW=S8?nd=TBUyH^szUGwHcZ_085TFwz#|m8>-DLDz_i63t}Q{&1Hz4#&BBM00Rg zVBLmTo3$&AFIBXyzJFV$-LXKdTj9!w1s4u$sTtwJ%L#eIW7Q-qMV*+xeM-%y0(?Xu zYf$T);aSqS%JCFk#=-}_oMlbLI6SL(vsS@VW3P{axttW?Aj^|nTNjt{WwB<@*PDZT z83dbE=PjR;JkTlb_0}gc$vw%DL8IuHL48?t7bk-p_2$2S%@_`iYL2H6r(tbXtG6$H zi1#UpOr)gY$kAjz^D_2qA(d?Drx*fE7ciOz|S65GQ?@VtM-pB2z zI4+D&hV8ICIAo>$0u9M+c}S*w#r~(Y`X!*Ot*s<>_$|Jy`Jtq%-UyXuOq-?62R=8(;>I?z9KdCKML;#{YLY$;T>XZm?=UMn_|2rJTDP1Hb8tg|jxd^v+7b=!NmtTqBeh&ZS#8&>3NHz5w>{Y4R_ zO^gPq`R-cbRMDwPNbP_#R>)zaj_`d(XF|e#kUT~iLdsnipk{POw`}Y61ZAD0nZ%DK z`9$<-)~~Drk;!X=k_bh1nq3~u>-~rbzMYZ?_?z4aK6~P}R|Rp=V)u!VrbLFxIW+2b z>QCbRY0tN4TkELh&c0Z?EZk3qPr_Z~pM`RmqbUOkJ-FMoK2VOdHC4y-G}8eV+DZWk zX6jN-&=s0$n)ykYm32Cz^-9AHW)kRCfBXP_Rx{TG3mN7#g=+BS3*~Hwshl1}_t0Tr z@>%){i8cncHw7ld83d}Tbd$lY)kp&6w=djR4OnT|iOe!>@!}5DO!8*$5^bG9=g)2C zhntFe*FYJuTv6y}J@zbU^Oo(_A470wLp;z+iI}Hu+#FvD9GC*|JoXx#vUsEWFMWzs zrZu`29dr4^OWAsvC}BUpF4b3865d`bCI=`twM+)7OHA!s+~FKJo5g*Z3)bGBekB6l z{^OH$w2KEi*_gGoh!}k-;;t>d zONzdN&YtPqo8~CDbOb*JqmAK3!_<^zKpEMCm1_Aw;5Ap z5mLu5wB~x0{)K=s#@QHe4QB^QHDEk8EK5WS~XtNf1f;f+>NG|?7@i{z{;oEixJ8NF5> zqrFoEMY^>gJf2r0h7)7!AZa0;Q)Gm-_udiHd6-r+nLkdP8Idjb7YZHg0a|P*pi7*?SHZmWTU_)ek9rzu5jNMxZ1-PQ*8;dpg0KMZ+ zvg<$xcKwT1PCU?+SNM$wAHJ2tf2-A$Hg|CNMu7i3u;2Rm|Lb+l{H9sv<-UiSxL|KC zp<+^oL`w;+0@uOD5|ltr1!It<>CyM9qAyLPU7^`<<=sZwJj}lcAO#Jed;j1|xZP-) z_$diC9(R?o{+&~-z0B_J_6ANFjEe%X=ZqU66Q?A1(h!AWTU?EZ3$shuPcfd!pqaK8 z!fD0;=)T-Z(rPPKxoI++8v5w=@#2 zMjXbSXl5Z|#_JGO8fUn|tFn|N+D7@TQwqfCT14gR8eKfo(XD8)29;&w))lNX3C4^C z4_yvO`*Vokel4~CYWw|m?mdP`6}1AN$VtBqzG;7rd!*;vK*TA97s|PqHCZ{xFnm)~ z9s2x4@urFRS56_BvH!qM3*$k#n1pR|IB6|zmWY+93=<3xqmsN1=9s}qAI$)aN{!JH zA_;b-#~mdM`1_d@qW?<#VVuI_28>DS-W;HRhS3j+m07d#0Xp|#ZnIhhr8t)5s_EE` zT3JNF4UnQUH9EOWEO^G^5&wflY#veqIXg;kE-My3<3l<9gfNQkP1q**CvbxQNd9i4 z?}rC`rg%nf{cI18sklEK1$F*5M?}!fAVS$8bbE-G#XWNyeA8y{>>3X2v0d-+Oj2Nm zDM~hDkKQMEUONW4)V08yH^lSkurW|St2O-qg*X|7z@2eK@Q#PRzc^?S&VF!iHkZ9r zQ|_p96s8ueJgP3de8T?u*X4X7*PB1c+u43Z4}DJ|zhVoT0A8Fiv)KyX%2cjV8ZN3c ztL25YZ~Q;dWu@}E_5AmW*7O3qy%ypGR;@9T0t)F($+h1UowgLH!l=2w zK!qu7u!lkB2db9ff@F80U3Y&HLxo6uuR{t-k=~4>KaMap`91+%-=X4x zPIjb`(iwV6mt`gQh|&>5t)M7K(0ED|DJt@k5JMGy`CcbL;4X9eMpYv9y3t4yjy&B0 zXf?}(|7;DEY^&|$+8O=?lHh`ed24Gb-U*!6TTaZ0@pw}Q7YzJ;?~UHyTPQ)J#Zvh? z@zWJEmhvLkp>o(em;{^vHcBnExu;CTR9eB;(I!)lr!hG6E{)ZFyun7Nb=JW@0qs@d zEkQlh4xOnd+KSSjO@HD@I=o=|<+>iix{rdun$Lsk$f(=9m_IWJCWN&~H&6?b*q;D~ z_z1*N#2($~+O|WY^B2XDwT~$_Z>S36GLjfaX(W-3%cth0B?O@ffccd9nP^2UYXi03 z4uGbbTuq5S1&7(wk?e{h zVAQ9y(!U+Xu-73g-D=uy!XCaY0}{*g46Aw(uj3Y^`bK2@ecVX7t+Z{Sba#VZYI$;U za)t(vXQ(p)x&2Z1>e|kteyh;gzRHrGHZFI%Py~Mt0qoEdxHKWd^)3)GmjLTWKW3do zAjEvy9GP>k;}a@@mp%Hf?5FySdRRTR601M)xPFMIdDtwb#x(F{<^lxbF(}O2M7WWp zl2Z1I|46W47x`fC9WM8*U=}&;9?~EtEz$n{MNV}jhKm(Yw$~vO&R{W4Hb*>XipJ>;XH2Jpx|a+wMXI;lt6wo3Z)Ljs`DHXyJ)$LIq``b zD^gxc6cys%uUQ7+5cWzYV*7mU@Rfg|8&gPjCfdIbLD}~qVEcDktbY!{zmfonO8n{L7g&g|Bl-aN0_nVe5{2&8e+`xB zMjki8%CJ(Aq9@AD?tZ1GGLZ5Aq1*=~L5L@!tSX&ponNexPDz*N=h8YKH9L-P81rF9{!7(z-F7_b$_>=@tomyjdThM!y<6Bae zY{vdG=_1{p8)N}8ioS;C@(dr@R_)}T5C%c>V|b~c;5LhRi;iAu8)R}ulL@=&s@Zk6 z>}ySWoQ>vDwvcTPx>kHaVbZ+SX}@rki*GH~J4+^t9PC z=u|fHt=14)lle{6cYvOX)mZ&GBJ2{g$@KN8b~e?65RAYOh7N;tzih~EAExjN@1q+I z%{fZHMf2P&Y=78aW10S)9?~lu7_`s|<`1A++aoC^NWXxm+jurhppAHvH?dRhvT4g} zhq=&!vD%Yows`SWp3OsVWit8a_qg>5DDv6w@3>Lm9=CAtDXgJv-m&d;~GjW^oz$Nk(#o z1@_a2@uE@10q#}vxN(esT?KbwBA8PA?NrPEpYyT)cg5-dgKbER+m`sAk2Ta?uU_9) zg!RR|*tAsgGaqGH!bakI{!w92PLLRFM>=soXI*OIYUm4;7fv+@-Rlppk~yYy-;f~Y zcJ%Gk`t85CQyCv0$GhmhL<<5aHHdw~BEFM9lm%|p%#Hbwp&mQodTollzGque(8vY{ zR52gtrQ4dcCO!$xA&Ru#v!AX@CL$(HRaHtn!s|1duc@egD!o=UGEWK_r5cS7tNhs` zXU)qVDM>CVNreLwc-GFA*S^Fo;8zo42_DKC(|j8o_}K(;FZ+tK^h}zcEzqyTWWgS@ zh9q-VNo7ZrCv?L8M>F4XBPFc`LGn%7C|ap&BD@1pRflYD?8kcG=Bv?7FhDcF#Y3#* zBRajkVLtbCw0g{{;BLZUXNXE4Z14wHVE*azZ*o4JS@ma$C)d8`c`ZbJk2~_fGvavN z!>{FFkFc8!sb3(TVQQgHCSQ14xZrpu4#;GuWJm0@kuVUqKsRotYGY2ARIOEe##N}v zbX>=47@whw*!`#5H)A98{>QVNI>*K~_FtOT@KY!+UcqjB1B4c-kBRlkrvGYy$QybV zF8{s^o4$h=|CZeN&(Hsd7yXB2N>uui`3|dpKDi%`*(GRz2+1RcH;9hQ4`lzsvXF{^ zASDO;(yU6hckQ&eg3FKILw=zn1_~wR^}Q~zbJj$#j2DQXx|*2syq}!7`gpznAoJzm zJ{9JZ${c8jVh$6aDWuQe$D)R<=VV3+B8O&3?z7tEs@|;vc)&p7En(D+ufG#Db6+i2 zG_pH>tN{ti&V+3C6i?=zx8Hu>Rb89an+j^Ca#Z|_`WR}?UZ%#yU8jLIFGa^8Qht-2 zPIzqsHkga93Dl`Ym)3uh-Nbi}_SsrnFPardtK(KG0R0Alo=5;j>-W%a zv;YBaW_n*32D(HTYQ0$f1D}mzt}0b00pREwqaDs63=9t4-W0$vOrgWA$;f-Z?&gN` z#Y@8Jh((?U{Aty(@Y^H#kv>kR!#)il7cQQrqnK(M8+N!FX;TKysz_yWVeZyih+bxz zPFhwq*I9wiJQZaX@R@Fd zhm)M^g4J!ocM&Sr#Je(})eKrZfmJTtsBOj#%QhS~p?;xq0xat>K!`S6yqJ+fOHe7RiPEXH z=n0VtGLibuH)7tE89ep3(GVosQpm zp|j;a@eEz7Rpe-uw=-^hN9oU9&rT-Yo*rL_J%lQb4~8PawCJ#I-}SFFF?tvaaBG!b zTBym%9f;9t*5>+-4c`T6gEj75YQhMztT$#gMLkh}wXQgjGilvp^{t|I(d@IA0>GVn zVpcietfni2yDnL&wq|Q@girp$h%7qMbnk`ys)1-$xqmNOeHiRAOobh0h4dia@LIh{ zy#XGd*48bZ$YIF~Nt-&b2;LJ)iLy;M0aw48LMd|`3NK3}exvO%Kva$Hkbmypq|qc`#aotE2e&8Cg`toXsxK7lp#v2NQs4T)#v(*T` z4V-l$BJ&{B?HBmT8)3|K-ss)Yn$YH3|v82T4{qFo{drP++b-XdQ8sW`iIaxs@bhmv(W2Fxcau^uSMsEK>Rj z73{pi-93B=GkRE^q(gv}Me`lRD$4u##NtahUMW~WV<_G(mZgpxEkT>ktO&T}AiKv) zYPQQC9FaFTI5u-gy3R1+TJ&fCfwY)wTXYdcPDt(be=m1EX>Vna?{aVX*1{P79o+jr zI=)23ZJRl{?>rL)3bcdo`T_?kA{z$wVkc$8Dd{}$~`4ejC5hO@{QnXc#T z0QlFBFY^6Xn)J?tY@wU`ojVNF&?|( zbnfCK%xS|Q_1F^Kz7K?C~u(8lI(naxFtb;QU!&?z02`H&FF z!mkS)m6y@=PwvK@>EsMeD+WefGIOsvHuV@0?F+bwogS6kg5}ae=zx=nP;tE?I({Q9 zVRtg!inDjc7#8DG$VPEZA`5Im)BVEC9nv_2iK;;wK}ioH&CPgGbexUQ@(Sj9_!r)kvXCJ%encU1>SYu&bJCU4kM% zu&#jOS{6FHo~6ie5+zx|y)N0k&eb>APMu|luTQ!uedH$Hsv?C|)pDP8od%Zf@L%DB z?d11_^zWLo_?E2r{+*gqwzl}c2v(iS;|kx#LLQem@jm+B5D2$HA>`r^fywY7wJ~#Z zlu(rd>NV}eigu2Sg3_d8bT4$Y1!1Cz(0o0K*t*bc)*B~uYRT4w>&?@r zUBxz}*FN1|;CfKaECVr%Gk{uFjmY}Z+SHu@@koWD{1&W1mY!%e<_Q}MIwi={u_m2rB<#9V4J9>?*vl5oRZfXJTmY|e!7f;(GLTw$3dyXdC-ur& zs_ZQKr0CpVi2L-7ErFzqvnpB^fdXWKiYzKQQQ2%ZnB1O5i8%H>MR9pfj2#q3(f2sp zVrO!56^9YP@>1p*qBZ4b(z8B}iwWo#QPzJfZ2n5J5;l5WWJQI2))jQh@YnAnpn|kj!GlSHn`h1%4Pf10 z#$`L|cVl)t_`K}u(j}W>gTh}T{@E_S>wj}-5oWCtG&&=!2_|H?_mnV%zl1v9mRA+J zCMJ^31?>7-WTFszA&y6w3_lSx!8<+n4o@pN{Lvn?<(T0BQ29+UM7(g`QwA~LQZnP4 zU<-r)B?xOkj>kLd9>>fmqNQU{&&ZyHsS0l7`|r20kw*Fg+V}Ep%kOXy>A!Ju{=wRr z>gIY{gR!3yX{l`P-^*cF>v;4mcY)877@BGh6?uPPO0p)^#==jixyOm%O^2i+HnD$i ze?W{vh|)s_^3w|j@ozPP_FI*1=|dX1LRy)u(_anX@r5O@{4qT2{jrrkJ8^;;`Yz`p z>!R$W?6kPNC|ix|@r2;3ey4=Td0YGEQ?Ht>j(7H!;}2=V^6W0W$^`7 zI4ep!?~O!v5~B<=*F@yi7{w_Ts5@e*KyKL4voF&)g4EC{VF$Szr8e2F46~Y@w1hMV zB%|OUt0FB_LN@$5!IPUVer2bGG~Q`Jtd_L+EQLyuIkjw*8Ta0}ElPt!T7GJ#Kxo*& zonOLfp)?We+vTM-Y)^7ym3oj22{2xeP&!pdpt(j%`AtU70i5Ar?K>M$lchY5>M(Uj~|*+YrLz+Z9N3Kui`=?Fe|1= zh!)mB7k+gDHRK;^CKd1GKRWJjSI>*YMszDj=op$RO-x?XI{$YHU5cHrjt6NIvle|B z#L$juDFK31N_xp**g>|YiJyMW_!Wp>UXUE`c*Np>XD~WQ6<0EWeTxkBn;XiVq$xQnv48#Lm*K9f1Q8ZhUc3t@ zaByP4iMp@`I;U1fwS$bkGAwxxx!D;{Fr(r!oG;(WaktP|&V_b?=8BQmip6Luj5$0| zhc~53_*^ZlbQ-2(Y8FF)29@X0^xnMcQ5Se~#b*hLhQt+n2DLTSmsT`OMuM0oSz=k* zm^XohSF%XMksLI`ycclL8ia^bIX9+^&a4uqXvT>sPv0wq!P{{4E3DjB=sm@V$Y7%! zC+sm1RYq9hN$~{yN{e7VltX_cA)c|!n;*q?dYXczgf!fg(noPLrnnxesgD==To z8kL8^Xe6-n;aMKLfz8PlRF#MSv?4>??F%vaeY|2;u^2((FqEY{<}^6LdJYlC1ZqB3 z2{oA5)w({3mp4GtYs<#=m=-G}^`WExESws{F`1^KHG35pCaemZYTNP4S&coDVz1)h z8*Z79OCNUVzXp0;MeWe`E?DxliQF|%2gv+p-JXPDdv`g^VtVM@?JFJ?P6J_C73sK& z0ASccOU!}Lgai6b!cl)%Gh6~G=;U>AUOIwkc2>p3YGZLOhFEDwM3HA02;!~cRX5T<+xEU;Np547z(7REiT>>AxDj?=02(=YF7$%UbodGTeWgW)mhUq%ohVGsscH}xZ zFvAmi7P59!*J~lG8ifrnwf6T!fOnxnfy+8QVkBu4a81qdeDepEiW>$<4BTR0#DoQW#Xh48w zkOr5#77d`5aa;OS*H+0?*2SoI*}r^XC-_7qOqyh=csx#Lg>hkQ;q_?!}lL-SJD0?H4&BRTO`(T7`&1=fH z0g9@7?8b;wGwu11oSm{o@(2a)+v}dEcFaqdFJr`Tp%QNrqmIDFSa17nefwd?;NaEU z(#gt`FJTu}HP<`XFin|1%8^^}AmpUB1EQQ$c0SzBm)=_Eg<(8417DwupI)rljtaNr zZ!AN8cyEV!L^3VFlg#OVE8?Kq_gdBKK8{@L9YI6kM5O`k4C2vLnrurQ>zRO>*pd){ zz3B0|ccsUkB^<*IiL?N3Kcj2iHMHJbD41!e)8V1H5xSTc=e~^O90+yHjLh1Wa+A!h zsoiZ6;mE2e)6``%fiuL#d5-M={fwoxF9fU!#-A*n=IWKM&w6fl-e<0p zdsn$Tzxt~Hkl3`0vvVNwF?#PRg}gj1OfgXZX(wfV=*t!t0bR$4n!F}W{m&0LlNF>A&2Jm-taK&Yln0GU5z zg!R9P+|Jc4c&$~?;e0^r=y@EmV%*K6r^IyM+Jo+v?U}Zaph@_=ol40*wb0{(PeHbw z>xTsnVu8b9`43^L!`Rw3ZM>{%%-%P=J3nCihI4UopHu_=f*oEV;eU>t>SB?$kzDv;~WH^`S`elYG z*-6@0jA_omI-bj}^^@vts~0>)LPgL8s+ErVUw*UB zn`>FfTXiWa>Yw|TgrdG!mqU0}+vBytAJ2b>*|<^jXExZ(40s1!Ut^ay;5%C{%nu$2 zbZvhO{fsa>86G*RgW~X&k394u-+}H!zIo7Z&};6f5()C}?n}|IG45FpuWdi9^=+;x zLEm@I&%xhMM?DW5^0LP-2JU1xXOkf`?vdP!_h6`9Lce+3LqXD#@fSzqSMJfQsX>po z@MJYcqzFT;M4JJ6KWrV@<4Ke*#febLn_ z>w@cZkC(cLHm<6wz6*Xncuo@WbSZYya>K>a#F$Q|dc{UKB&?WBzW0e+N)Jg&82PLQ zj>?XA{Sm?dxM?5gAqP{{fM{M1+0cp!ZwQS$68d&|B}{jputRd}xdt{nA9Q$@l1OjN zwPBRPEZM+OjDqt}$}*WW&=}cSj4W?1h_)37eOx+ZRA=B&{?i+b>yYDNWV}UbYk=)Q zP>aH+hvg2lDxPoOodbaFV4spi`Gh}cc6QhgZ_BsdPLKH=`oZCekYCCWnS}93Y+G@} za!L0GzeR8iHDvG>isJs$IH~dIu+43%6sAgXN?`AKa`S4wTD&sOfq!yL+ooa`CK*a5zP0v<5_Vz--GC62C>eyW3Jv6(Yq3-K%NWL6Xy!!|CEm|)Mz%W>E z8o}p}6cv@1RSD1*Et%D)=A1BlM=CzT0YvvVP&fOXK}KZ{D8k`P?nVeeRZiT)*pEM% z=FU_qeKs+p%;7KvQdJQe#e{H?@5!Jesxq)<)e46sH(6w?SKJ)^FkwkxQ^6~{Jy>!L z?-0%cPaPB9Qg7@EGm^=Q4d9)a>IGPIM!an+Kj=s0)XsqsL{vM{mxvH33e!z(xV#6{ z`Ke{~DFS`$k{wC!l};Mz_P4M{A9wg2cg30(J!DExlI6~DOy0jNOTs*m^C+sdVS>|8 zKQbY|-cZxXWaaYAPh&a(6n8nMC$E#4Ax1dG1^7U`kbyP)eNt<$z# zeKqf8_zvmg@OpT5%}K7@-KjUNJ3r7^Rf>FD;loeDy{U_?lNQ`5X zXHyC%i3!D^8iGWLS`tcKhJXqJ60@d+&adg%I-N)y%VpG8B@euw1mA7gj8|K2kPH>G~2^m))x1XKx$48W}sSyxP{S^wVRF|HV zSk#xKrLp;$DhJ9vDqaY%EILEM2Ie>ubBPA(l^rv|ENJbGe@9V+j@`0`*N(IrXNb+t z205{qs|n4g|1uYbn6-A<23RGq1$3V8EW-~7xP9?syH(BlAPhezomNa`j4br9Fz z)=~FT)xlItaCuX3-KK2-mJdlf2&(s_-7;NWiW66eC_FeWNyhAkMMLJM8Npo?+Ozl3 zBevk_Vd?ByzGrXwCsVhv6s(Tp+}Ppw3y4LwYlS3-2BbkP8R^(QNOla#O~s?%vbkoe zBg7QnQr#UJByEJVsd2iM+}^v!s~Q^P|b?a;Rxpn}(?tsFwEWKETpFp4?3BvCi5gy4)HQYE#UD<7N|{(C=aHd(2(eQrshhDxlelF8qM>` z?!0>eag8!)0GMz9P1*xxHa$t6>2EWBNqBCD`#9Y24Ad)Tu`6xK*_p{(M;4Dbj0LQy z%O9jFpEv&AJWr7I^R~32?HCc~v6<%wf!D(hX9T6A8GT&3cqG%Ov}t_I^NJRnkCk?) z40aie{3tP3S-krhh($@gBH7JJs$BGY!0`02RLo%7Lxm;5!mS%1%yUC9v`4f>ieE4H z#l!OqX^|s43*g(cuhNd>V;JW(jq>3?_#5Zu!R`cQIIF)&sZ$kIb0@Y*8LZGeMsTds znrK>jN8=W3HoVhJ8%0!N;w!@&QL5YHfg-HJ%tTy__Huju0)K2$Wl{|%)5`w*z1p=m zqk(I6-12zJ=u`GR8QMYSslPAtZ@0EflK#cS$XoUTvUzAD5C{~PM{Op$pD8|ftE~PX z{g+?P+@KCOnx(#?cP%8e!)k;X?=ysdA>^SgL=k26OVx%=wa~L|(d(mYv!{8dcze6j z_h|LI<1^Y z5rl?QRzUbq<^7^<3Nrw4iZW@%LvB%uj&Gr+rJ~GIy%hkFrYABRAUnS$q%D0>;?e0F z*YC*NTZCx#;`B%J6dANYbnJuKuiyJ@rPo1!W(yoV9-N|E*bi?ZPSQpCp{sJ6NZ*CU zkKUycUA-@@e-CT-x2UC~bWalsYqBGg!6ArFWmEw1t)0(NT zZ%ah9P*p#+ogxb4pG<{n=s1{w6yf)5Pnc7k->i4J$D=#oy!(LeDbH6emaBR=LFm?bmTzLCYIaUSX9i+(Np3Ech~* zZHTPZ`qMW7@!C0m)ySk|8>=iz9uk3a={c)1BmX_(iy>YbGwBzbB70ITRD;4)n5Re3 zv3feudeh@Wv$Z^3LRkfij>W8`O&Xe0GmItv={wtBH*eWd&MAov7wPat zRX+eoZInHV$FwzpEE#?ASl&^}UDi!0=un=cDFEG_WE^xJtRnhKeVAkBcPLe5t$F(B zdMxkAZQBM_DexyTjp?KgPItFnTep?d7nJi;%7+2_B3wz#V@$6<-6N=m@0Eb_ma<*2 ztl1m5s--y1ew_AvXWGOBMlS{P^oSw+WJ3-`l?LTUxly?Y@u^I6d#dM}QeckO61;u5 z*oLSY({aV(R;c;E4J-16B^vd3ZXp@#!TXInjaahq0>{!8;$%ZPqW!!dTfeZcQFyZ1 z>`NnKReAcFyh{VoCo(Ecg&r#L7$AT&J50!dWuZCSI$7O;2*rs6tQS_bbKP5x$#Btj|uuR!tp8n*%I3T z#I*o#zgxZ75dLNmV{k-117H-Xi89zDKYCfrph%G{*9i8aW)#fi>{Od&bOn&EF~ftt z+7Pq>z)@g8x%{iNrNriHjL8#Tcz|$oqk6D3K2kKbzn0Hlx!8MjN0IXyEo3x@M3g3*q)7 zf=$>mM3McVz#U|myVoDXx{f+xFGNmwCa95_dZ&z|Bvtyn?%{DPH&dD&SoE3s&_z0x z;~M43AnS-z%h+87s-#;(dqrM5{(uxI-x``q{p*WxUWkEWpcdlud)Nt*NWi7ZdDIrC z_*E;|%V30~wZFY1*p<%OpJEBchiO-F5;>!XwzZz1kddp zLZ#w8zx>=scB@Ztd0c#j?z|9PpBNz*-EK)g4%Ib=AD#i#u%c_fz|}vELP1yJH;%_G zBIz&kcdB@=G(LXklqV+FuusvJHyD%Dgh&vGat^kil{edhO2WkgZP$cFd57ALEfGEm zA{ooH`(!1zw_6z}?LjLUIq8nv7yXTl)rjW5#`YLa&C~01FLasqF-bD~i?@MUFJQU& zSK^=jJ}|QE;-6WsfAZ7xKB+J(n3l$B6d_yYh*tf=XlZKuwE1eZmsuk&H(f!fH*$*- z=8VRBrHYD*9hKoEhI<&FNX$4HtbcL+-fc8Vrj^C=axFkI+|CN6am>_(t&OL%n-LR| zXL0(#i=SzkCh-Z&b)93uyM`NMyhTR&m(~3<4n_DN8BWx=fa0lu|1Wo@HZ_;#WnRA` zFqhUtg=`xdz#g5)lATxmS6KhH?*TGIn9kY;$7BRg7*A5X&9B*MBPkOrMH%aA`I`Ybng+8#5_=~W4X{{&s zp|@|-*oP4uBv0IA7toH!!d(J7dy@Ny_DjwVaC~P;D|)N5{HHp?{K9H-kn(a+Nk${B z{~CaG+Xi)9`xa=0zdbJ0|5IlAA7J1gd)GgZAo4rry6_u?XS4cB)X(^@9Ed(@ps{>e z$;(f|5Hm3q2K9j6W_=e0u=dNMOQhZ68_T_L_>>Y5@dZ<#gj*R+J$2&S-1*dXk7=Ic zjqk;++de;1`r?`E$jeg1i2Mzpa9gs94gq1K#1G6!EvdaUQY3boUDqWoRNM3Rt;Ks? z|EIDufroPId>lu~1>khSb`Z}t=!`zW%eR6~<(n0XDNNTWf@b}bdxZX%T;np@o~ z(jpSKP@+_Hy(&v?mP+^bo{8~rj4|)&GoP_^zP~ePd(Lw_=l4G;fL^t`kw|tiVN}*L z&USsIm7Jk{c%)>R9*x(!@`lVOub%65yrN#sRP#t;S$u}Rid7@pCX|9Mh#q$0D>wVy z`ks^`e)vp6hryw}6~U=;H&Wd3y($#i=Gfb3f0I37m4Co6CP43!Z(x-N`X5osp1tms ze%c3}6kDxdVi;xvDg5Kk=TLkvqlYWfL@LvboWsVW+U`h~6rz383{`x@j1I34O>A9u z(OF!w(7xw%ab7W5$HpM}K%Mf9$YGm+jk=D;r>mTjH9CcgYjXwbLtab1OI>AUy5g{C zP+qH{X$!n|DOCvC7Z1h zLb#ijLmCEVemlBALG`lx+>j-CJM z{h@xv#Js&KqkRhBOy1ko*g1^9E1Qrp(!v^?%anZ^SMoN$#p>Wa#eciXlWFTD1ES($ zH&V4-ltR*P33%k}#G;=mJh;o#As5=>+aU21_EK|k|9@jb19hYPwg}ym-xdxYfL#h6fHhzqHN zYkcGRSE)zjf>t}WM{V$3mj0`ekRsBM<`vXf`EFyewPD2G@^lO3*a69qCC@P{(GljB zE`En-IER~AWiM9AR!j4{Uk=#yOt;C+#-Op<(;EA!y|FJxLO9WFXBeaS><3EcaP&*( zzo~{Dmbt3xpYxQDABzsC^mB-j_Y4fixsHDJ@(yo#wk?L1;9ELcW8OHntM9o~DYh@8 zuPLcd@fq&(3&k|dQ~tzN!->&}k}9$L;?Dn7wRQCA2?Hg$*v-@qnn$E{Tf&&2xYXs+ z_LD(>AN;Ua#b*3^n-u!hwIU%`r>>7{oU5eb3t#wbl-7!T;3rgjJ92pfS?_rEApy7Y zS9*>cy#}|gS#39hFKYTV!#^#)X~5`sPNONB&!GZCky=_LR?Jg)3KK5)P-{=pn-RD7 z|KV4UFm2h_XU&_LWA-qv&zCnd!%S81{Fg%;N=8@A{_{GzSaQPzz=BLBF>Q^P|%BeNnwjwq79i}r|@D4J&`6WOqN zeY4?>G@M^Cmc%VrU_17)(9zUH(3Np8iJwT-!F6ng7(=exsw5C*3 z$^`UBU)w+AjcY3CzPctu1(Qyh&@|3*@)ERG>GdpMP7qb49B)w7x`l3AJg7h}x;0XH zOs6_OLo-O7?~z)8VTm_**C=p9U)bW;@Ae%!8vjrG)&fz`lo;@0df-oa--Bn=Is4xK z#g*H=;%p+BqtiVPugD@`558mx$YcUuh-p4BSDQ-0sDU59vNdxwQMcM|u4!j8JDY#` z79(TupPA21fk;WyiB1KNgrKIg*_v#(GB2B@A%#i?(d?zypHcFT)lO%(98W6yOD8?n5M)czS{wx5WqGz2>X%9Wh`BayD&NpQEt}Go42UWTnwA<_|%>>Wwvn$^e4>v zR$*TaG$)R%LWU<(G(D&=EHM@W|V)P*a|Qn z4hw+b3E`aZ&|L|Ph28KG?7aw1*qPfsFcbDhMwm-!oR~lMl;&Nk!8XJQb&MP8{HDZk z@nIuXL@4_N7sa1zs|pLiwv~uL@+mF^IG9+%O0bI^qVyq&3ni{R?O;vVhz!xpO5sA2 zlPwu61)H)UQWF_mNO7=eft6tY3qjn5ACL*xp{QoJiP>sQd;1H>C zumXmzaWkg(sYz|Yx`GcxA$*%sF8G{}N5KsPpCLiSqRSQ*W8W6=(*p?eRqY(+kLsBF zECF0j_>T|>v%g_sCZ}r@ymgC^g`4J*x!=fzKLNa*i0Hg+o}&Y=W@mJx1uo<878fG( z+vDkl-FzEfaG9BzS*t|m?iMT2se)iLW5(_odEUJ)I~zW5%Y{PefPe47&D?g75rz66 D613UA literal 0 HcmV?d00001 diff --git a/2020-Robot/gradle/wrapper/gradle-wrapper.properties b/2020-Robot/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..d050f17 --- /dev/null +++ b/2020-Robot/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/2020-Robot/gradlew b/2020-Robot/gradlew new file mode 100644 index 0000000..2fe81a7 --- /dev/null +++ b/2020-Robot/gradlew @@ -0,0 +1,183 @@ +#!/usr/bin/env sh + +# +# Copyright 2015 the original author or authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +## +## 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" "-Xms64m"' + +# 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 or MSYS, switch paths to Windows format before running java +if [ "$cygwin" = "true" -o "$msys" = "true" ] ; 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=`expr $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" + +exec "$JAVACMD" "$@" diff --git a/2020-Robot/gradlew.bat b/2020-Robot/gradlew.bat new file mode 100644 index 0000000..9618d8d --- /dev/null +++ b/2020-Robot/gradlew.bat @@ -0,0 +1,100 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@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" "-Xms64m" + +@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/2020-Robot/settings.gradle b/2020-Robot/settings.gradle new file mode 100644 index 0000000..81f96ab --- /dev/null +++ b/2020-Robot/settings.gradle @@ -0,0 +1,27 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2020' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/2020-Robot/src/main/deploy/example.txt b/2020-Robot/src/main/deploy/example.txt new file mode 100644 index 0000000..bb82515 --- /dev/null +++ b/2020-Robot/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 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/2020-Robot/src/main/java/frc/robot/Constants.java b/2020-Robot/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..cf0c82e --- /dev/null +++ b/2020-Robot/src/main/java/frc/robot/Constants.java @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { +} diff --git a/2020-Robot/src/main/java/frc/robot/Main.java b/2020-Robot/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..4d2951a --- /dev/null +++ b/2020-Robot/src/main/java/frc/robot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.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/2020-Robot/src/main/java/frc/robot/Robot.java b/2020-Robot/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..bd546a8 --- /dev/null +++ b/2020-Robot/src/main/java/frc/robot/Robot.java @@ -0,0 +1,113 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/2020-Robot/src/main/java/frc/robot/RobotContainer.java b/2020-Robot/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..f60f0d1 --- /dev/null +++ b/2020-Robot/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import frc.robot.commands.ExampleCommand; +import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems and commands are defined here... + private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + + private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); + + + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // An ExampleCommand will run in autonomous + return m_autoCommand; + } +} diff --git a/2020-Robot/src/main/java/frc/robot/commands/ExampleCommand.java b/2020-Robot/src/main/java/frc/robot/commands/ExampleCommand.java new file mode 100644 index 0000000..42c175f --- /dev/null +++ b/2020-Robot/src/main/java/frc/robot/commands/ExampleCommand.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * An example command that uses an example subsystem. + */ +public class ExampleCommand extends CommandBase { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ExampleSubsystem m_subsystem; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ExampleCommand(ExampleSubsystem subsystem) { + m_subsystem = subsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2020-Robot/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/2020-Robot/src/main/java/frc/robot/subsystems/ExampleSubsystem.java new file mode 100644 index 0000000..b2585b9 --- /dev/null +++ b/2020-Robot/src/main/java/frc/robot/subsystems/ExampleSubsystem.java @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ExampleSubsystem extends SubsystemBase { + /** + * Creates a new ExampleSubsystem. + */ + public ExampleSubsystem() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/2020-Robot/vendordeps/WPILibNewCommands.json b/2020-Robot/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..d7bd9b0 --- /dev/null +++ b/2020-Robot/vendordeps/WPILibNewCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "2020.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} From bce118d655a37c3ad88c1d43bada28f25e530cf1 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Thu, 16 Jan 2020 22:08:52 -0500 Subject: [PATCH 002/121] Adding the first set of files Copied Utils from 2019. Added some joystick buttons. --- .../src/main/java/frc/robot/Constants.java | 19 -- 2020-Robot/src/main/java/frc/robot/Robot.java | 2 + .../main/java/frc/robot/RobotContainer.java | 57 ---- .../org/mayheminc/robot2020/Constants.java | 28 ++ .../mayheminc/robot2020/RobotContainer.java | 111 ++++++++ .../org/mayheminc/util/AndJoystickButton.java | 23 ++ .../util/DisabledOnlyJoystickButton.java | 26 ++ .../util/EnabledOnlyJoystickButton.java | 26 ++ .../main/java/org/mayheminc/util/History.java | 59 ++++ .../mayheminc/util/JoystickAxisButton.java | 91 ++++++ .../org/mayheminc/util/JoystickPOVButton.java | 34 +++ .../org/mayheminc/util/MB1340Ultrasonic.java | 31 +++ .../org/mayheminc/util/MayhemTalonSRX.java | 153 ++++++++++ .../org/mayheminc/util/ObjectListener.java | 159 +++++++++++ .../org/mayheminc/util/ObjectLocation.java | 58 ++++ .../java/org/mayheminc/util/PidCycle.java | 10 + .../java/org/mayheminc/util/PidTuner.java | 164 +++++++++++ .../org/mayheminc/util/PidTunerObject.java | 13 + .../mayheminc/util/RangeFinder_GP2D120.java | 60 ++++ .../org/mayheminc/util/SchedulerManager.java | 107 +++++++ .../main/java/org/mayheminc/util/Utils.java | 262 ++++++++++++++++++ 2020-Robot/vendordeps/Phoenix.json | 180 ++++++++++++ 22 files changed, 1597 insertions(+), 76 deletions(-) delete mode 100644 2020-Robot/src/main/java/frc/robot/Constants.java delete mode 100644 2020-Robot/src/main/java/frc/robot/RobotContainer.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/History.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/Utils.java create mode 100644 2020-Robot/vendordeps/Phoenix.json diff --git a/2020-Robot/src/main/java/frc/robot/Constants.java b/2020-Robot/src/main/java/frc/robot/Constants.java deleted file mode 100644 index cf0c82e..0000000 --- a/2020-Robot/src/main/java/frc/robot/Constants.java +++ /dev/null @@ -1,19 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be - * declared globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants { -} diff --git a/2020-Robot/src/main/java/frc/robot/Robot.java b/2020-Robot/src/main/java/frc/robot/Robot.java index bd546a8..59bb8c2 100644 --- a/2020-Robot/src/main/java/frc/robot/Robot.java +++ b/2020-Robot/src/main/java/frc/robot/Robot.java @@ -7,6 +7,8 @@ package frc.robot; +import org.mayheminc.robot2020.RobotContainer; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; diff --git a/2020-Robot/src/main/java/frc/robot/RobotContainer.java b/2020-Robot/src/main/java/frc/robot/RobotContainer.java deleted file mode 100644 index f60f0d1..0000000 --- a/2020-Robot/src/main/java/frc/robot/RobotContainer.java +++ /dev/null @@ -1,57 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot; - -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.Command; - -/** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot - * (including subsystems, commands, and button mappings) should be declared here. - */ -public class RobotContainer { - // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); - - private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); - - - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - // Configure the button bindings - configureButtonBindings(); - } - - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a - * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - } - - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous - return m_autoCommand; - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java new file mode 100644 index 0000000..66c8abd --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean constants. This class should not be used for any other + * purpose. All constants should be declared globally (i.e. public static). Do + * not put anything functional in this class. + * + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the constants are needed, to reduce verbosity. + */ +public final class Constants { + + public final class Joysticks { + public static final int DRIVER_GAMEPAD = 0; + public static final int DRIVER_JOYSTICK = 1; + public static final int OPERATOR_GAMEPAD = 2; + public static final int OPERATOR_JOYSTICK = 3; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java new file mode 100644 index 0000000..cdc332c --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -0,0 +1,111 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020; + +import org.mayheminc.util.*; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; +import frc.robot.commands.ExampleCommand; +import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems and commands are defined here... + private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + + private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); + + private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); + private final Joystick DRIVER_STICK = new Joystick(Constants.Joysticks.DRIVER_JOYSTICK); + private final Button DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(DRIVER_STICK, 1); + private final Button DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(DRIVER_STICK, 1); + private final Button DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(DRIVER_STICK, 2); + private final Button DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(DRIVER_STICK, 3); + private final Button DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(DRIVER_STICK, 4); + private final Button DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(DRIVER_STICK, 5); + private final Button DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(DRIVER_STICK, 6); + private final Button DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 7); + private final Button DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(DRIVER_STICK, 8); + private final Button DRIVER_STICK_BUTTON_NINE = new JoystickButton(DRIVER_STICK, 9); + private final Button DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 10); + private final Button DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 11); + public final int DRIVER_STICK_X_AXIS = 0; + public final int DRIVER_STICK_Y_AXIS = 1; + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + confiugreDriverStickButtons(); + confiugreDriverPadButtons(); + confiugreOperatorStickButtons(); + confiugreOperatorPadButtons(); + + } + + private void confiugreDriverStickButtons() { + + // DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); + // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); + + // // adjust auto parameters + // DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(1)); + // DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(-1)); + // DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(-1)); + // DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(1)); + + // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner + // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); + + // // zero elements that require zeroing + // DRIVER_STICK_BUTTON_EIGHT.whenPressed(new DriveZeroGyro()); + // DRIVER_STICK_BUTTON_NINE.whenPressed(new Wait(0)); + + } + + private void confiugreDriverPadButtons() { + } + + private void confiugreOperatorStickButtons() { + } + + private void confiugreOperatorPadButtons() { + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // An ExampleCommand will run in autonomous + return m_autoCommand; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java new file mode 100644 index 0000000..2c4da30 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java @@ -0,0 +1,23 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.*; + +public class AndJoystickButton extends Button { + GenericHID joystick1; + GenericHID joystick2; + int buttonNumber1; + int buttonNumber2; + + public AndJoystickButton(GenericHID joystick1, int buttonNumber1, GenericHID joystick2, int buttonNumber2) { + this.joystick1 = joystick1; + this.buttonNumber1 = buttonNumber1; + this.joystick2 = joystick2; + this.buttonNumber2 = buttonNumber2; + } + + public boolean get() { + return joystick1.getRawButton(buttonNumber1) && joystick2.getRawButton(buttonNumber2); + } + +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java new file mode 100644 index 0000000..a76ab1c --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java @@ -0,0 +1,26 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.*; + +/** + * + * @author Team1519 + */ +public class DisabledOnlyJoystickButton extends Button { + + private GenericHID joystick; + private int buttonNumber; + private DriverStation ds; + + public DisabledOnlyJoystickButton(GenericHID joystick, int buttonNumber) { + this.joystick = joystick; + this.buttonNumber = buttonNumber; + ds = DriverStation.getInstance(); + } + + public boolean get() { + return joystick.getRawButton(buttonNumber) && ds.isDisabled(); + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java new file mode 100644 index 0000000..9bd9d4a --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java @@ -0,0 +1,26 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.*; + +/** + * + * @author Team1519 + */ +public class EnabledOnlyJoystickButton extends Button { + + private GenericHID joystick; + private int buttonNumber; + private DriverStation ds; + + public EnabledOnlyJoystickButton(GenericHID joystick, int buttonNumber) { + this.joystick = joystick; + this.buttonNumber = buttonNumber; + ds = DriverStation.getInstance(); + } + + public boolean get() { + return joystick.getRawButton(buttonNumber) && ds.isEnabled(); + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/History.java b/2020-Robot/src/main/java/org/mayheminc/util/History.java new file mode 100644 index 0000000..b91707f --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/History.java @@ -0,0 +1,59 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.DriverStation; + +public class History { + private static final int HISTORY_SIZE = 50; + + private double time[] = new double[HISTORY_SIZE]; + private double azimuth[] = new double[HISTORY_SIZE]; + private int index = 0; + + public History() { + // ensure there is at least one element in the history + add(-1.0, 0.0); // make a fictitious element at t=-1 seconds, with heading of 0.0 degrees + } + + public void add(double t, double az) { + + time[index] = t; + azimuth[index] = az; + + index++; + if (index >= HISTORY_SIZE) { + index = 0; + } + } + + public double getAzForTime(double t) { + double az = azimuth[index]; + int i = index - 1; + int count = 0; + + // if (t < 0) { + // DriverStation.reportError("Negative time in history", false); + // return 0.0; // no negative times. + // } + + while (i != index) { + if (i < 0) { + i = HISTORY_SIZE - 1; + } + + if (time[i] <= t) { + az = azimuth[i]; + break; + } + + i--; + count++; + if (count > HISTORY_SIZE) { + DriverStation.reportError("Looking too far back", false); + az = azimuth[index]; + break; + } + } + + return az; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java b/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java new file mode 100644 index 0000000..0657aa4 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java @@ -0,0 +1,91 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.*; + +/** + * + * @author Team1519 + */ +public class JoystickAxisButton extends Button { + public static final int BOTH_WAYS = 1; + public static final int POSITIVE_ONLY = 2; + public static final int NEGATIVE_ONLY = 3; + + private static final double AXIS_THRESHOLD = 0.2; + + private Joystick joystick; + private Joystick.AxisType axis; + private int axisInt; + private int direction; + + private double getAxis(Joystick.AxisType axis) { + switch (axis) { + case kX: + return joystick.getX(); + case kY: + return joystick.getY(); + case kZ: + return joystick.getZ(); + case kThrottle: + return joystick.getThrottle(); + case kTwist: + return joystick.getTwist(); + default: + // Unreachable + return 0.0; + } + } + + public JoystickAxisButton(Joystick stick, Joystick.AxisType axis) { + this(stick, axis, BOTH_WAYS); + } + + public JoystickAxisButton(Joystick stick, Joystick.AxisType axis, int direction) { + joystick = stick; + this.axis = axis; + this.direction = direction; + } + + public JoystickAxisButton(Joystick stick, int axis) { + this(stick, axis, BOTH_WAYS); + } + + public JoystickAxisButton(Joystick stick, int axis, int direction) { + joystick = stick; + this.axis = null; + axisInt = axis; + this.direction = direction; + } + + public boolean get() { + switch (direction) { + case BOTH_WAYS: + if (axis != null) { + return Math.abs(getAxis(axis)) > AXIS_THRESHOLD; + } else { + return Math.abs(joystick.getRawAxis(axisInt)) > AXIS_THRESHOLD; + } + + case POSITIVE_ONLY: + if (axis != null) { + return getAxis(axis) > AXIS_THRESHOLD; + } else { + return joystick.getRawAxis(axisInt) > AXIS_THRESHOLD; + } + + case NEGATIVE_ONLY: + if (axis != null) { + return getAxis(axis) < -AXIS_THRESHOLD; + } else { + return joystick.getRawAxis(axisInt) < -AXIS_THRESHOLD; + } + } + + return false; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java b/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java new file mode 100644 index 0000000..a061fc4 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java @@ -0,0 +1,34 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.*; + +/** + * @author Team1519 + */ +public class JoystickPOVButton extends Button { + public static final int NORTH = 0; + public static final int NORTHEAST = 45; + public static final int EAST = 90; + public static final int SOUTHEAST = 135; + public static final int SOUTH = 180; + public static final int SOUTHWEST = 225; + public static final int WEST = 270; + public static final int NORTHWEST = 315; + + private Joystick joystick; + private int desiredPOV; + + public JoystickPOVButton(Joystick stick, int newDesiredPOV) { + joystick = stick; + desiredPOV = newDesiredPOV; + } + + public boolean get() { + return (joystick.getPOV() == desiredPOV); + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java b/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java new file mode 100644 index 0000000..76442a5 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java @@ -0,0 +1,31 @@ +package org.mayheminc.util; +import edu.wpi.first.wpilibj.AnalogInput; + +/* + * The Ultrasonic sensor being used is an MB1340. + * The conversion factor being used is (5 volts) / (1024 per cm). + */ + +public class MB1340Ultrasonic { + AnalogInput analogUltrasonic; + final static double voltage = 5.0; + final static double maxADCBins = 1024; + + public MB1340Ultrasonic(int analogPort) { + analogUltrasonic = new AnalogInput(analogPort); + } + + /** + * Get the Distance in cm. + * Full scale is 1024 cm. The minimum is 25cm. + * @return + */ + public double getDistance() { + return (analogUltrasonic.getAverageVoltage() / voltage) * maxADCBins; + } + + public double getDistanceInInches() { + return getDistance() / 2.54; + } + +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java new file mode 100644 index 0000000..d605650 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -0,0 +1,153 @@ +package org.mayheminc.util; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.*; + +public class MayhemTalonSRX extends TalonSRX { + + ControlMode controlMode; + double p; + double i; + double d; + double f; + + public MayhemTalonSRX(int deviceNumber) { + super(deviceNumber); + + this.configNominalOutputForward(0.0, 0); + this.configNominalOutputReverse(0.0, 0); + this.configPeakOutputForward(1.0, 0); + this.configPeakOutputReverse(-1.0, 0); + + this.setNeutralMode(NeutralMode.Coast); + +// this.configContinuousCurrentLimit(0, 0); +// this.configPeakCurrentLimit(0, 0); +// this.configPeakCurrentDuration(0, 0); +// this.configForwardLimitSwitchSource(RemoteLimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled, 0, 0); +// this.configForwardSoftLimitEnable(false, 0); + + // copied from CTRE Example: https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java/Current%20Limit/src/org/usfirst/frc/team217/robot/Robot.java#L37 +// this.configPeakCurrentLimit(80, 10); +// this.configPeakCurrentDuration(60000, 10); /* this is a necessary call to avoid errata. */ +// this.configContinuousCurrentLimit(40, 10); +// this.enableCurrentLimit(true); /* honor initial setting */ + + } + + public ErrorCode config_kP(int slot, double value, int timeout) + { + p = value; + return super.config_kP(slot, value, timeout); + } + + public ErrorCode config_kI(int slot, double value, int timeout) + { + i = value; + return super.config_kI(slot, value, timeout); + } + + public ErrorCode config_kD(int slot, double value, int timeout) + { + d = value; + return super.config_kD(slot, value, timeout); + } + public ErrorCode config_kF(int slot, double value, int timeout) + { + f = value; + return super.config_kF(slot, value, timeout); + } + + public double getP() {return p;} + public double getI() {return i;} + public double getD() {return d;} + public double getF() {return f;} + + public void changeControlMode(ControlMode mode) { + controlMode = mode; + } + + public void set(int deviceID) { + this.set(controlMode, deviceID); + } + + public void setFeedbackDevice(FeedbackDevice feedback) { + this.configSelectedFeedbackSensor(feedback, 0, 1000); + } + + public void reverseSensor(boolean b) { + + } + + public void configNominalOutputVoltage(float f, float g) { + this.configNominalOutputForward(f/12.0, 1000); + this.configNominalOutputReverse(g/12.0, 1000); + } + + public void configPeakOutputVoltage(double d, double e) { + this.configPeakOutputForward(d/12.0, 1000); + this.configPeakOutputReverse(e/12.0, 1000); + + } + + public void setPID(double wheelP, double wheelI, double wheelD, double wheelF, int i, double m_voltageRampRate, + int j) { + this.config_kP(pidProfile, wheelP , 1000); + this.config_kI(pidProfile, wheelI , 1000); + this.config_kD(pidProfile, wheelD , 1000); + this.config_kF(pidProfile, wheelF , 1000); + } + + public double getSetpoint() { + return 0; + } + + public double getError() { + return this.getClosedLoopError(0); + } + + public float getOutputVoltage() { + return (float) this.getMotorOutputVoltage(); + } + + int pidProfile; + public void setProfile(int pidSlot) { + pidProfile = pidSlot; + } + + public void setPID(double pDown, double iDown, double dDown) { + setPID(pDown, iDown, dDown, 0.0, 0, 0.0, 0); + } + + public void setVoltageRampRate(double d) { + // Need to convert volts per second to time + this.configClosedloopRamp(0, 0); + } + + public void enableControl() { + + } + + public void setPosition(int zeroPositionCount) { + this.setSelectedSensorPosition(zeroPositionCount, 0, 1000); + } + + public int getPosition() { + return this.getSelectedSensorPosition(0); + } + + public double getSpeed() { + return this.getSelectedSensorVelocity(0); + } + + public void setEncPosition(int i) { + setPosition(i); + } + + public double get() { + return this.getOutputCurrent(); + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java b/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java new file mode 100644 index 0000000..5cae99b --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java @@ -0,0 +1,159 @@ +package org.mayheminc.util; + +import java.util.*; +import java.io.*; +import java.net.*; +import java.nio.*; + +public class ObjectListener extends Thread { + protected static final int MAX_OBJECTS_PER_FRAME = 20; + protected static final int MAYHEM_MAGIC = 0x1519B0B4; + protected static final int MAX_BUFFER = 1500; + protected static final int DEFAULT_PORT = 5810; + + private DatagramSocket socket; + private DatagramPacket packet; + private ByteBuffer buffer; + private int lastFrame = 0; + private ArrayList objList; + private Callback callback = null; + + public interface Callback { + public void objectListenerCallback(int frame, ArrayList objList); + } + + public ObjectListener() throws SocketException { + this(DEFAULT_PORT); + } + + public ObjectListener(int port) throws SocketException { + super("ObjectListener-" + port); + + socket = new DatagramSocket(port); + + byte[] byteBuffer = new byte[MAX_BUFFER]; + packet = new DatagramPacket(byteBuffer, byteBuffer.length); + buffer = ByteBuffer.wrap(byteBuffer); + + objList = new ArrayList(); + } + + public int getLastFrame() { + return lastFrame; + } + + public List getObjectList() { + return objList; + } + + public void setCallback(Callback callback) { + this.callback = callback; + } + + public void run() { + String name = super.getName(); + long lastTimestamp = 0; + + while (true) { + // Receive new datagram + try { + socket.receive(packet); + buffer.rewind(); + } catch (IOException e) { + System.err.println(super.getName() + " encountered an error"); + e.printStackTrace(); + System.err.println(super.getName() + " aborting"); + break; + } + + // Abort if told to do so + if (Thread.interrupted()) + break; + + // Validate packet + int magic = buffer.getInt(); + if (magic != MAYHEM_MAGIC) { + System.err.println(name + ": invalid packet received (magic == 0x" + Integer.toHexString(magic) + ")"); + continue; + } + + // Get information about the update + int frame = buffer.getInt(); + long timestamp = buffer.getLong(); + + // Check for out-of-date data + if (timestamp <= lastTimestamp) { + System.err.println(name + ": timestamp for new frame #" + frame + " (" + timestamp + ") is not newer than that for previous frame #" + lastFrame + " (" + lastTimestamp + "); rejecting out-of-date data"); + lastTimestamp = timestamp; + continue; + } + if (frame <= lastFrame) { + System.err.println(name + ": frame #" + frame + " is earlier than existing frame #" + lastFrame + "; did object detection service restart?"); + } + + // Get list of all objects involved + ArrayList objList = new ArrayList(); + for (int i = 0; i < MAX_OBJECTS_PER_FRAME; i++) { + // Pull the next object from the buffer + ObjectLocation loc = new ObjectLocation(buffer); + + // As soon as one object is none, so are the rest + if (loc.type == ObjectLocation.ObjectTypes.OBJ_NONE) + break; + + // Add object to our list + objList.add(loc); + } + + // Update the list of objects + this.objList = objList; + lastFrame = frame; + lastTimestamp = timestamp; + + // Invoke callback, if applicable + if (callback != null) { + callback.objectListenerCallback(frame, objList); + } + } + + // Clean up + socket.close(); + } + + // Sample implementation for testing and demonstration purposes + public static void main(String[] args) { + ObjectListener listener; + Callback callback = new ObjectListener.Callback() { + public void objectListenerCallback(int frame, ArrayList objList) { + System.out.println("Received notification about objects in frame #" + frame); + for (ObjectLocation loc: objList) { + System.out.println(" " + loc); + } + } + }; + + // Create the listener + try { + listener = new ObjectListener(); + } catch (SocketException e) { + e.printStackTrace(); + return; + } + + // Use the callback implementation + listener.setCallback(callback); + + // Begin listening + System.out.println("Starting object listener...\n"); + listener.start(); + + // Wait forever -- notifications will come from callback + while (true) { + try { + Thread.sleep(600); + } catch (InterruptedException e) { + break; + } + } + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java b/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java new file mode 100644 index 0000000..a903461 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java @@ -0,0 +1,58 @@ +package org.mayheminc.util; + +import java.nio.*; +import java.text.DecimalFormat; + +public class ObjectLocation { + public enum ObjectTypes { + OBJ_NONE, + OBJ_CUBE, + OBJ_SCALE_CENTER, + OBJ_SCALE_BLUE, + OBJ_SCALE_RED, + OBJ_SWITCH_RED, + OBJ_SWITCH_BLUE, + OBJ_PORTAL_RED, + OBJ_PORTAL_BLUE, + OBJ_EXCHANGE_RED, + OBJ_EXCHANGE_BLUE, + OBJ_BUMPERS_RED, + OBJ_BUMPERS_BLUE, + OBJ_BUMPERS_CLASS13, + OBJ_BUMPERS_CLASS14, + OBJ_BUMPERS_CLASS15, + OBJ_BUMPERS_CLASS16, + OBJ_BUMPERS_CLASS17, + OBJ_BUMPERS_CLASS18, + OBJ_BUMPERS_CLASS19, + OBJ_BUMPERS_CLASS20, + OBJ_EOL, + }; + + public ObjectTypes type; + public float x; + public float y; + public float width; + public float height; + public float probability; + + public ObjectLocation() { + type = ObjectTypes.OBJ_NONE; + x = y = width = height = probability = 0; + } + + public ObjectLocation(ByteBuffer buffer) { + type = ObjectTypes.values()[buffer.getInt()]; + x = (float)buffer.getInt() / Integer.MAX_VALUE; + y = (float)buffer.getInt() / Integer.MAX_VALUE; + width = (float)buffer.getInt() / Integer.MAX_VALUE; + height = (float)buffer.getInt() / Integer.MAX_VALUE; + probability = (float)buffer.getInt() / Integer.MAX_VALUE; + } + + public String toString() { + DecimalFormat df = new DecimalFormat("0.00"); + + return type.name() + "@" + df.format(x) + "x" + df.format(y) + "+" + df.format(width) + "x" + df.format(height) + "[" + df.format(probability * 100) + "%]"; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java b/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java new file mode 100644 index 0000000..cb5fc71 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java @@ -0,0 +1,10 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class PidCycle extends InstantCommand { + public void initialize() { + // Robot.pidTuner.cyclePid(); + } + +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java b/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java new file mode 100644 index 0000000..bd1975b --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java @@ -0,0 +1,164 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * This is a class to help tune a PID. Another class that extends + * IPidTunerObject is passed in. This interface defines methods that are used to + * set the P-I-D values. This class gets joystick buttons that define the + * different options. - Button 1: cycle through P-I-D values - Button 2: + * increase the value. - Button 3: decrease the value. - Button 4: cycle through + * the amount the value changes: 10, 1, .1, .01, .001, .0001 This class needs to + * be called for updateSmartDashboard(). + * + * @author User + * + */ +public class PidTuner extends InstantCommand { + Button m_PidCycle; + Button m_Inc; + Button m_Dec; + Button m_ValueCycle; + PidTunerObject m_pidObj; + + enum PidCycle { + P, I, D, F + }; + + int m_cycle; + int m_position; + + // Remember the buttons and call this object when they are pressed. + // remember the PID Object so we can get/set the PID values. + public PidTuner(final Button b1, final Button b2, final Button b3, final Button b4, final PidTunerObject obj) { + + b1.whenPressed(this); + b2.whenPressed(this); + b3.whenPressed(this); + b4.whenPressed(this); + + m_PidCycle = b1; + m_ValueCycle = b2; + m_Inc = b3; + m_Dec = b4; + + m_pidObj = obj; + } + + public boolean runsWhenDisabled() { + return true; + } + + // Run the 'instant command'. Determine which command was pressed. + public void initialize() { + if (m_PidCycle.get()) { + RunCycle(); + } + if (m_Inc.get()) { + RunInc(); + } + if (m_Dec.get()) { + RunDec(); + } + if (m_ValueCycle.get()) { + RunValue(); + } + } + + // set the P, I, or D that we are changing + public void RunCycle() { + // System.out.println("PID Tuner: RunCycle"); + m_cycle++; + m_cycle = m_cycle % 4; + } + + String getCycleStr() { + final String str[] = { "P", "I", "D", "F" }; + + return str[m_cycle]; + } + + // calculate the amount to increment, get the value, apply the amount, set the + // new value + public void RunInc() { + // System.out.println("PID Tuner: RunInc"); + final double amount = calculateAmount(); + + double value = getValue(); + value = value + amount; + setValue(value); + } + + // if m_position is 1, return 10, 0 returns 1, -1 returns 0.1, etc. + double calculateAmount() { + final double retval = Math.pow(10, m_position); + return retval; + } + + // based on the cycle, get the P, I, or D. + double getValue() { + switch (m_cycle) { + case 0: + return m_pidObj.getP(); + case 1: + return m_pidObj.getI(); + case 2: + return m_pidObj.getD(); + case 3: + return m_pidObj.getF(); + } + return 0.0; + } + + // based on the cycle, set the P, I, or D. + void setValue(final double d) { + switch (m_cycle) { + case 0: + m_pidObj.setP(d); + break; + case 1: + m_pidObj.setI(d); + break; + case 2: + m_pidObj.setD(d); + break; + case 3: + m_pidObj.setF(d); + break; + } + } + + // calculate the amount to decrement, get the value, apply the amount, set the + // new value + public void RunDec() { + // System.out.println("PID Tuner: RunDec"); + final double amount = calculateAmount(); + + double value = getValue(); + value = value - amount; + setValue(value); + } + + // Change the amount we are decrementing or incrementing + public void RunValue() { + // System.out.println("PID Tuner: RunValue"); + + m_position--; + if (m_position < -4) { + m_position = 1; // 10.1234 + } + } + + public void updateSmartDashboard() { + SmartDashboard.putNumber("PID Tuner P", m_pidObj.getP()); + SmartDashboard.putNumber("PID Tuner I", m_pidObj.getI()); + SmartDashboard.putNumber("PID Tuner D", m_pidObj.getD()); + SmartDashboard.putNumber("PID Tuner F", m_pidObj.getF()); + + SmartDashboard.putString("PID Tuner Cycle", getCycleStr()); + SmartDashboard.putNumber("PID Tuner Amount", calculateAmount()); + } + +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java b/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java new file mode 100644 index 0000000..d9a59d0 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java @@ -0,0 +1,13 @@ +package org.mayheminc.util; + +public interface PidTunerObject { + + double getP(); + double getI(); + double getD(); + double getF(); + void setP(double d); + void setI(double d); + void setD(double d); + void setF(double d); +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java b/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java new file mode 100644 index 0000000..061fcbc --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java @@ -0,0 +1,60 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.AnalogInput; + +// http://media.digikey.com/pdf/Data%20Sheets/Sharp%20PDFs/GP2D120.pdf + +public class RangeFinder_GP2D120 { + + private AnalogInput m_channel; + private static final int HISTORY_BUFFER_SIZE = 5; + private double m_historyBuffer[]=new double[HISTORY_BUFFER_SIZE]; + private int m_historyBufferIndex; + private double m_filteredVoltage; + + public RangeFinder_GP2D120(int channel, int index) { + m_channel = new AnalogInput(channel); + m_filteredVoltage = 0.0; + } + + public void periodic() { + double min = m_historyBuffer[0]; + m_historyBuffer[m_historyBufferIndex]= m_channel.getVoltage(); //return channel.getVoltage(); + m_historyBufferIndex++; + + if (m_historyBufferIndex>=HISTORY_BUFFER_SIZE) { + m_historyBufferIndex=0; + } + + for (int i=1; i= OBJECT_IS_SEEN_VOLTAGE; + } + + // had been 0.80 prior to 31 Jan + private static final double OBJECT_IS_CLOSE_VOLTAGE = 0.80; + public boolean isObjectClose() { + return getFilteredVoltage() >= OBJECT_IS_CLOSE_VOLTAGE; + } + + // added on 11 Feb 2015 to allow "floating off tote" + private static final double OBJECT_IS_VERY_CLOSE_VOLTAGE = 1.60; + public boolean isObjectVeryClose() { + return getFilteredVoltage() >= OBJECT_IS_VERY_CLOSE_VOLTAGE; + } + +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java b/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java new file mode 100644 index 0000000..26300ab --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java @@ -0,0 +1,107 @@ +/** + * Copyright (c) 2015-2019 Team 501 - The PowerKnights. All Rights Reserved. + * Open Source Software - May be modified and shared by FRC teams. The code must + * be accompanied by the Team 501 BSD license file in the root directory of the + * project. You may also obtain a copy of it from the following: + * http://www.opensource.org/licenses/bsd-license.php. + * + * See (Git) repository metadata for author and revision history for this file. + **/ + +package org.mayheminc.util; + +// import org.slf4j.Logger; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableType; +import edu.wpi.first.networktables.NetworkTableValue; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +// import riolog.RioLogger; + +/** + * + */ +public class SchedulerManager // implements ITelemetryProvider +{ + + /* Our classes logger */ + // @SuppressWarnings("unused") + // private static final Logger logger = + // RioLogger.getLogger(SchedulerManager.class.getName()); + + /** Singleton instance of class for all to use **/ + private static SchedulerManager ourInstance; + /** Name of our subsystem **/ + private final static String myName = "Running Commands";// TelemetryNames.Scheduler.name; + + /** + * Constructs instance of the subsystem. Assumed to be called before any usage + * of the subsystem; and verifies only called once. Allows controlled startup + * sequencing of the robot and all it's subsystems. + **/ + public static synchronized void constructInstance() { + // SmartDashboard.putBoolean(TelemetryNames.Scheduler.status, false); + + if (ourInstance != null) { + throw new IllegalStateException(myName + " Already Constructed"); + } + ourInstance = new SchedulerManager(); + + // SmartDashboard.putBoolean(TelemetryNames.Scheduler.status, true); + } + + /** + * Returns the singleton instance of the subsystem in the form of the + * Interface that is defined for it. If it hasn't been constructed yet, + * throws an IllegalStateException. + * + * @return singleton instance of subsystem + **/ + public static SchedulerManager getInstance() { + if (ourInstance == null) { + throw new IllegalStateException(myName + " Not Constructed Yet"); + } + return ourInstance; + } + + private final NetworkTable liveWindow; + private final NetworkTable ungrouped; + private final NetworkTable scheduler; + private final StringBuilder names; + + public SchedulerManager() { + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + liveWindow = inst.getTable("LiveWindow"); + ungrouped = liveWindow.getSubTable("Ungrouped"); + scheduler = ungrouped.getSubTable("Scheduler"); + + names = new StringBuilder(); + } + + // @Override + public void updateTelemetry() { + NetworkTableEntry namesEntry = scheduler.getEntry("Names"); + NetworkTableValue namesValue = namesEntry.getValue(); + if ((namesValue == null)) { + SmartDashboard.putString("Current Commands", "namesValue is null"); + return; + } + + if ((namesValue.getType() != NetworkTableType.kStringArray)) { + + SmartDashboard.putString("Current Commands", "namesValue is " + namesValue.getType()); + return; + } + + names.setLength(0); + for (String name : namesValue.getStringArray()) { + names.append(name).append(": "); + } + + SmartDashboard.putString("Current Commands", names.toString()); + } + +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/Utils.java b/2020-Robot/src/main/java/org/mayheminc/util/Utils.java new file mode 100644 index 0000000..edcabda --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/Utils.java @@ -0,0 +1,262 @@ +/* + * Utility class with useful methods in it + */ +package org.mayheminc.util; + +/** + * + * @author Team1519 + */ +public class Utils { + + public static double oneDecimalPlace(double d) { + return ((double) ((int) (d * 10))) / 10; + } + + public static double twoDecimalPlaces(double d) { + return ((double) ((int) (d * 100))) / 100; + } + + public static double threeDecimalPlaces(double d) { + return ((double) ((int) (d * 1000))) / 1000; + } + + public static double fourDecimalPlaces(double d) { + return ((double) ((int) (d * 10000))) / 10000; + } + + /** + * Returns the arc tangent of an angle, in the range of + * -Math.PI/2 through + * Math.PI/2. Special cases:

A result must be within 1 + * ulp of the correctly rounded result. Results must be semi-monotonic. + * + * @param a - the value whose arc tangent is to be returned. + * @return the arc tangent of the argument. + */ + public static double atan(double a) { + // Special cases. + if (Double.isNaN(a)) { + return Double.NaN; + } + + if (a == 0.0) { + return a; + } + + // Compute the arc tangent. + boolean negative = false; + boolean greaterThanOne = false; + int i = 0; + + if (a < 0.0) { + a = -a; + negative = true; + } + + if (a > 1.0) { + a = 1.0 / a; + greaterThanOne = true; + } + + double t; + + for (; a > PIover12; a *= t) { + i++; + t = a + ATAN_CONSTANT; + t = 1.0 / t; + a *= ATAN_CONSTANT; + a--; + } + + double aSquared = a * a; + + double arcTangent = aSquared + 1.4087812; + arcTangent = 0.55913709 / arcTangent; + arcTangent += 0.60310578999999997; + arcTangent -= 0.051604539999999997 * aSquared; + arcTangent *= a; + + for (; i > 0; i--) { + arcTangent += PIover6; + } + + if (greaterThanOne) { + arcTangent = PIover2 - arcTangent; + } + + if (negative) { + arcTangent = -arcTangent; + } + + return arcTangent; + } + /** + * Constant for PI divided by 2. + */ + public static final double PIover2 = Math.PI / 2; + /** + * Constant for PI divided by 4. + */ + public static final double PIover4 = Math.PI / 4; + /** + * Constant for PI divided by 6. + */ + public static final double PIover6 = Math.PI / 6; + /** + * Constant for PI divided by 12. + */ + public static final double PIover12 = Math.PI / 12; + /** + * Constant used in the + * atan calculation. + */ + private static final double ATAN_CONSTANT = 1.732050807569; + + /** + * Converts rectangular coordinates (x, y) to polar (r, theta). This + * method computes the phase theta by computing an arc tangent of y/x + * in the range of -pi to pi. Special cases:

  • If + * either argument is + * NaN, then the result is + * NaN.
  • If the first argument is positive zero and the + * second argument is positive, or the first argument is positive and finite + * and the second argument is positive infinity, then the result is positive + * zero.
  • If the first argument is negative zero and the second argument + * is positive, or the first argument is negative and finite and the second + * argument is positive infinity, then the result is negative zero.
  • If + * the first argument is positive zero and the second argument is negative, + * or the first argument is positive and finite and the second argument is + * negative infinity, then the result is the + * double value closest to pi.
  • If the first argument + * is negative zero and the second argument is negative, or the first + * argument is negative and finite and the second argument is negative + * infinity, then the result is the + * double value closest to -pi.
  • If the first + * argument is positive and the second argument is positive zero or negative + * zero, or the first argument is positive infinity and the second argument + * is finite, then the result is the + * double value closest to pi/2.
  • If the first + * argument is negative and the second argument is positive zero or negative + * zero, or the first argument is negative infinity and the second argument + * is finite, then the result is the + * double value closest to -pi/2.
  • If both arguments + * are positive infinity, then the result is the double value closest to + * pi/4.
  • If the first argument is positive infinity and the + * second argument is negative infinity, then the result is the double value + * closest to 3*pi/4.
  • If the first argument is negative infinity + * and the second argument is positive infinity, then the result is the + * double value closest to -pi/4.
  • If both arguments are negative + * infinity, then the result is the double value closest to -3*pi/4. + *

A result must be within 2 ulps of the correctly rounded result. + * Results must be semi-monotonic. + * + * @param y - the ordinate coordinate + * @param x - the abscissa coordinate + * @return the theta component of the point (r, theta) in + * polar coordinates that corresponds to the point (x, y) in Cartesian + * coordinates. + */ + public static double atan2(double y, double x) { + // Special cases. + if (Double.isNaN(y) || Double.isNaN(x)) { + return Double.NaN; + } else if (Double.isInfinite(y)) { + if (y > 0.0) // Positive infinity + { + if (Double.isInfinite(x)) { + if (x > 0.0) { + return PIover4; + } else { + return 3.0 * PIover4; + } + } else if (x != 0.0) { + return PIover2; + } + } else // Negative infinity + { + if (Double.isInfinite(x)) { + if (x > 0.0) { + return -PIover4; + } else { + return -3.0 * PIover4; + } + } else if (x != 0.0) { + return -PIover2; + } + } + } else if (y == 0.0) { + if (x > 0.0) { + return y; + } else if (x < 0.0) { + return Math.PI; + } + else { + return 0; + } + } else if (Double.isInfinite(x)) { + if (x > 0.0) // Positive infinity + { + if (y > 0.0) { + return 0.0; + } else if (y < 0.0) { + return -0.0; + } + } else // Negative infinity + { + if (y > 0.0) { + return Math.PI; + } else if (y < 0.0) { + return -Math.PI; + } + } + } else if (x == 0.0) { + if (y > 0.0) { + return PIover2; + } else if (y < 0.0) { + return -PIover2; + } + else { + // Should not get her with y test above + return 0; + } + } + + + // Implementation a simple version ported from a PASCAL implementation: + // http://everything2.com/index.pl?node_id=1008481 + + double arcTangent; + + // Use arctan() avoiding division by zero. + if (Math.abs(x) > Math.abs(y)) { + arcTangent = atan(y / x); + } else { + arcTangent = atan(x / y); // -PI/4 <= a <= PI/4 + + if (arcTangent < 0) { + arcTangent = -PIover2 - arcTangent; // a is negative, so we're adding + } else { + arcTangent = PIover2 - arcTangent; + } + } + + // Adjust result to be from [-PI, PI] + if (x < 0) { + if (y < 0) { + arcTangent = arcTangent - Math.PI; + } else { + arcTangent = arcTangent + Math.PI; + } + } + + return arcTangent; + } + + public static boolean isBetween(double value, double upper, double lower){ + return value < upper && value > lower; + } +} diff --git a/2020-Robot/vendordeps/Phoenix.json b/2020-Robot/vendordeps/Phoenix.json new file mode 100644 index 0000000..a633555 --- /dev/null +++ b/2020-Robot/vendordeps/Phoenix.json @@ -0,0 +1,180 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.17.4", + "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.17.4" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.17.4" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.17.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.17.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.17.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.17.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.17.4", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.17.4", + "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.17.4", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.17.4", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.17.4", + "libName": "CTRE_PhoenixDiagnostics", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.17.4", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.17.4", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.17.4", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ] +} \ No newline at end of file From a8e4c7b0244b62b18f5250218aff66a37b20f9ec Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Fri, 17 Jan 2020 22:04:48 -0500 Subject: [PATCH 003/121] copied the joystick deffinitions into 2020 --- .../org/mayheminc/robot2020/Constants.java | 28 ++++++ .../mayheminc/robot2020/RobotContainer.java | 96 +++++++++++++++++++ 2 files changed, 124 insertions(+) diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java index 66c8abd..ebba44a 100644 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java @@ -18,6 +18,34 @@ * wherever the constants are needed, to reduce verbosity. */ public final class Constants { + public final class GAMEPAD_AXIS { + public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; + public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; + public static final int GAMEPAD_F310_LEFT_TRIGGER = 2; + public static final int GAMEPAD_F310_RIGHT_TRIGGER = 3; + public static final int GAMEPAD_F310_RIGHT_X_AXIS = 4; + public static final int GAMEPAD_F310_RIGHT_Y_AXIS = 5; + } + + public final class GAMEPAD_BUTTION { + public static final int GAMEPAD_F310_A_BUTTON = 1; + public static final int GAMEPAD_F310_B_BUTTON = 2; + public static final int GAMEPAD_F310_X_BUTTON = 3; + public static final int GAMEPAD_F310_Y_BUTTON = 4; + public static final int GAMEPAD_F310_LEFT_BUTTON = 5; + public static final int GAMEPAD_F310_RIGHT_BUTTON = 6; + public static final int GAMEPAD_F310_BACK_BUTTON = 7; + public static final int GAMEPAD_F310_START_BUTTON = 8; + public static final int GAMEPAD_F310_LEFT_STICK_BUTTON = 9; + public static final int GAMEPAD_F310_RIGHT_STICK_BUTTON = 10; + } + + public final class OPERATOR_PAD_AXIS { + public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; + public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; + public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; + public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; + } public final class Joysticks { public static final int DRIVER_GAMEPAD = 0; diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java index cdc332c..9f6db4a 100644 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -32,7 +32,72 @@ public class RobotContainer { private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); + + @SuppressWarnings("unused") + private final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger + @SuppressWarnings("unused") + private final Button DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger + @SuppressWarnings("unused") + private final Button DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left Stick Trigger + @SuppressWarnings("unused") + private final Button DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right Stick Trigger + + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); + + private final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); + private final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); + private final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); + private final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); + + private final Button CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, + // plus + // left + // top + // trigger + private final Button CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // A=Green, + // plus + // left + // top + // trigger + + // Driver Control Modes + @SuppressWarnings("unused") + private final Button TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); + @SuppressWarnings("unused") + private final Button TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); + + // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO + // NOT USE HERE!!! + private final Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); + private final JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + Constants.GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + private final JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + Constants.GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + + @SuppressWarnings("unused") + private final Button DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); + private final Button DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); + private final Button DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1); // Green "A" button + private final Button DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button + private final Button DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button + private final Button DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button + private final Joystick DRIVER_STICK = new Joystick(Constants.Joysticks.DRIVER_JOYSTICK); + private final Button DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(DRIVER_STICK, 1); private final Button DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(DRIVER_STICK, 1); private final Button DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(DRIVER_STICK, 2); @@ -48,6 +113,37 @@ public class RobotContainer { public final int DRIVER_STICK_X_AXIS = 0; public final int DRIVER_STICK_Y_AXIS = 1; + public static final Joystick OPERATOR_PAD = new Joystick(Constants.Joysticks.OPERATOR_GAMEPAD); + private static final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); + private static final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); + private static final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); + private static final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); + private static final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); + private static final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); + private static final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); + private static final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); + private static final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); + private static final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); + @SuppressWarnings("unused") + private static final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); + private static final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); + @SuppressWarnings("unused") + private static final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); + + // Operator Control Buttons + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ From 359c514e632a9d622a707a8a051c728223d31567 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 18 Jan 2020 13:27:54 -0500 Subject: [PATCH 004/121] added talon constants. --- .../org/mayheminc/robot2020/Constants.java | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java index ebba44a..5168b40 100644 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java @@ -18,6 +18,32 @@ * wherever the constants are needed, to reduce verbosity. */ public final class Constants { + + public final class Talon { + public static final int DRIVE_RIGHT_A = 1; + public static final int DRIVE_RIGHT_B = 2; + public static final int DRIVE_LEFT_A = 3; + public static final int DRIVE_LEFT_B = 4; + + public static final int SHOOTER_WHEEL = 5; + public static final int SHOOTER_FEEDER = 6; + public static final int SHOOTER_TURRET = 7; + public static final int SHOOTER_HOOD = 8; + + public static final int INTAKE_ROLLERS = 9; + public static final int INTAKE_EXTENDER = 10; + + public static final int MAGAZINE_TURNTABLE = 11; + + public static final int CLIMBER_WINCH_LEFT = 12; + public static final int CLIMBER_WINCH_RIGHT = 13; + + public static final int CLIMBER_WALKER_LEFT = 14; + public static final int CLIMBER_WALKER_RIGHT = 15; + + public static final int CONTROL_PANEL_ROLLER = 16; + } + public final class GAMEPAD_AXIS { public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; From ed9cc01a47b340050554d4a167d489d389a81515 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 18 Jan 2020 14:29:05 -0500 Subject: [PATCH 005/121] Adding the drive subsystem. --- .../org/mayheminc/robot2020/Constants.java | 14 +- .../mayheminc/robot2020/subsystems/Drive.java | 656 ++++++++++++++++++ .../subsystems/PIDHeadingCorrection.java | 19 + .../robot2020/subsystems/PIDHeadingError.java | 28 + 2020-Robot/vendordeps/navx_frc.json | 34 + 5 files changed, 744 insertions(+), 7 deletions(-) create mode 100644 2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java create mode 100644 2020-Robot/vendordeps/navx_frc.json diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java index 5168b40..8a0659a 100644 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java @@ -20,12 +20,12 @@ public final class Constants { public final class Talon { - public static final int DRIVE_RIGHT_A = 1; - public static final int DRIVE_RIGHT_B = 2; - public static final int DRIVE_LEFT_A = 3; - public static final int DRIVE_LEFT_B = 4; + public static final int DRIVE_RIGHT_A = 1; // high current + public static final int DRIVE_RIGHT_B = 2; // high current + public static final int DRIVE_LEFT_A = 3; // high current + public static final int DRIVE_LEFT_B = 4; // high current - public static final int SHOOTER_WHEEL = 5; + public static final int SHOOTER_WHEEL = 5; // high current public static final int SHOOTER_FEEDER = 6; public static final int SHOOTER_TURRET = 7; public static final int SHOOTER_HOOD = 8; @@ -35,8 +35,8 @@ public final class Talon { public static final int MAGAZINE_TURNTABLE = 11; - public static final int CLIMBER_WINCH_LEFT = 12; - public static final int CLIMBER_WINCH_RIGHT = 13; + public static final int CLIMBER_WINCH_LEFT = 12; // high current + public static final int CLIMBER_WINCH_RIGHT = 13; // high current public static final int CLIMBER_WALKER_LEFT = 14; public static final int CLIMBER_WALKER_RIGHT = 15; diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java new file mode 100644 index 0000000..5b6e268 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -0,0 +1,656 @@ +package org.mayheminc.robot2020.subsystems; + +import com.kauailabs.navx.frc.*; +import org.mayheminc.util.History; + +import edu.wpi.first.wpilibj.*; +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import org.mayheminc.robot2019.Robot; +import org.mayheminc.robot2019.RobotMap; +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.Utils; + +public class Drive extends Subsystem { + + History headingHistory = new History(); + + // Brake modes + public static final boolean BRAKE_MODE = true; + public static final boolean COAST_MODE = false; + + // PID loop variables + private PIDController m_HeadingPid; + private PIDHeadingError m_HeadingError; + private PIDHeadingCorrection m_HeadingCorrection; + private boolean m_HeadingPidPreventWindup = false; + private static final int LOOPS_GYRO_DELAY = 10; + + // Talons + private final MayhemTalonSRX leftFrontTalon = new MayhemTalonSRX(RobotMap.LEFT_FRONT_TALON); + private final MayhemTalonSRX leftRearTalon = new MayhemTalonSRX(RobotMap.LEFT_REAR_TALON); + private final MayhemTalonSRX rightFrontTalon = new MayhemTalonSRX(RobotMap.RIGHT_FRONT_TALON); + private final MayhemTalonSRX rightRearTalon = new MayhemTalonSRX(RobotMap.RIGHT_REAR_TALON); + + // Sensors + private AHRS Navx; + + // Driving mode + private boolean m_speedRacerDriveMode = true; // set by default + + // NavX parameters + private double m_desiredHeading = 0.0; + private boolean m_useHeadingCorrection = true; + private static final double HEADING_PID_P_FOR_HIGH_GEAR = 0.030; + private static final double HEADING_PID_P_FOR_LOW_GEAR = HEADING_PID_P_FOR_HIGH_GEAR / 2.0; + private static final double kToleranceDegreesPIDControl = 0.2; + + // Drive parameters + public static final double DISTANCE_PER_PULSE = 3.14 * 8.0 * 36 / 42 / (250 * 4); // pi * diameter * (gear ratio) / + // (counts per rev) + private boolean m_closedLoopMode = false; + private double m_maxWheelSpeed = 1.0; // set to 1.0 as default for "open loop" percentVBus drive + private double m_voltageRampRate = 48.0; + + private double m_initialWheelDistance = 0.0; + private int m_iterationsSinceRotationCommanded = 0; + private int m_iterationsSinceMovementCommanded = 0; + + private boolean autoAlign = false; + + // Targeting + // private double TARGET_ALIGNED = -0.4; + + /*********************************** + * INITIALIZATION + **********************************************************/ + + public Drive() { + try { + /* Communicate w/navX MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* + * See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for + * details. + */ + Navx = new AHRS(SPI.Port.kMXP); + Navx.reset(); + } catch (RuntimeException ex) { + DriverStation.reportError("Error instantiating navX MXP: " + ex.getMessage(), true); + System.out.println("Error loading navx."); + } + + // create a PID Controller that reads the heading error and outputs the heading + // correction. + m_HeadingError = new PIDHeadingError(); + m_HeadingError.m_Error = 0.0; + m_HeadingCorrection = new PIDHeadingCorrection(); + m_HeadingPid = new PIDController(HEADING_PID_P_FOR_HIGH_GEAR, 0.000, 0.04, m_HeadingError, m_HeadingCorrection, + 0.020 /* period in seconds */); + m_HeadingPid.setInputRange(-180.0f, 180.0f); + m_HeadingPid.setContinuous(true); // treats the input range as "continous" with wrap-around + m_HeadingPid.setOutputRange(-.50, .50); // set the maximum power to correct twist + m_HeadingPid.setAbsoluteTolerance(kToleranceDegreesPIDControl); + + // confirm all four drive talons are in coast mode + leftFrontTalon.setNeutralMode(NeutralMode.Coast); + leftRearTalon.setNeutralMode(NeutralMode.Coast); + rightFrontTalon.setNeutralMode(NeutralMode.Coast); + rightRearTalon.setNeutralMode(NeutralMode.Coast); + + // set rear talons to follow their respective front talons + leftRearTalon.changeControlMode(ControlMode.Follower); + leftRearTalon.set(leftFrontTalon.getDeviceID()); + + rightRearTalon.changeControlMode(ControlMode.Follower); + rightRearTalon.set(rightFrontTalon.getDeviceID()); + + // the left motors move the robot forwards with positive power + // but the right motors are backwards. + leftFrontTalon.setInverted(true); + leftRearTalon.setInverted(true); + rightFrontTalon.setInverted(false); + rightRearTalon.setInverted(false); + + // sensor phase is reversed, since there are 3 reduction stages in the gearbox + leftFrontTalon.setSensorPhase(true); + leftRearTalon.setSensorPhase(true); + rightFrontTalon.setSensorPhase(true); + rightRearTalon.setSensorPhase(true); + } + + public void init() { + // reset the NavX + zeroHeadingGyro(0.0); + + // talon closed loop config + configureCanTalon(leftFrontTalon); + configureCanTalon(rightFrontTalon); + } + + public void zeroHeadingGyro(double headingOffset) { + Navx.zeroYaw(); + setHeadingOffset(headingOffset); + + DriverStation.reportError("heading immediately after zero = " + getHeading() + "\n", false); + + m_desiredHeading = 0.0; + + SmartDashboard.putString("Trace", "Zero Heading Gyro"); + + // restart the PID controller loop + resetAndEnableHeadingPID(); + } + + public void initDefaultCommand() { + // setDefaultCommand(new SpeedRacerDrive()); + } + + private void configureCanTalon(MayhemTalonSRX talon) { + double wheelP = 1.5; + double wheelI = 0.0; + double wheelD = 0.0; + double wheelF = 1.0; + + talon.setFeedbackDevice(FeedbackDevice.QuadEncoder); + + talon.reverseSensor(false); + talon.configNominalOutputVoltage(+0.0f, -0.0f); + talon.configPeakOutputVoltage(+12.0, -12.0); + + if (m_closedLoopMode) { + talon.changeControlMode(ControlMode.Velocity); + m_maxWheelSpeed = 270; + } else { + talon.changeControlMode(ControlMode.PercentOutput); + m_maxWheelSpeed = 1.0; + } + + talon.setPID(wheelP, wheelI, wheelD, wheelF, 0, m_voltageRampRate, 0); + + // talon.enableControl(); + + DriverStation.reportError("setWheelPIDF: " + wheelP + " " + wheelI + " " + wheelD + " " + wheelF + "\n", false); + } + + /** + * Set the "Brake Mode" behavior on the drive talons when "in neutral" + * + * @param brakeMode - true for "brake in neutral" and false for "coast in + * neutral" + */ + public void setBrakeMode(boolean brakeMode) { + + NeutralMode mode = (brakeMode) ? NeutralMode.Brake : NeutralMode.Coast; + + leftFrontTalon.setNeutralMode(mode); + leftRearTalon.setNeutralMode(mode); + rightFrontTalon.setNeutralMode(mode); + rightRearTalon.setNeutralMode(mode); + } + + // *********************** CLOSED-LOOP MODE ******************************** + + public void toggleClosedLoopMode() { + if (!m_closedLoopMode) { + setClosedLoopMode(); + } else { + setOpenLoopMode(); + } + } + + public void setClosedLoopMode() { + m_closedLoopMode = true; + // reconfigure the "master" drive talons now that we're in closed loop mode + configureCanTalon(leftFrontTalon); + configureCanTalon(rightFrontTalon); + } + + public void setOpenLoopMode() { + m_closedLoopMode = false; + // reconfigure the "master" drive talons now that we're in open loop mode + configureCanTalon(leftFrontTalon); + configureCanTalon(rightFrontTalon); + } + + // ********************* ENCODER-GETTERS ************************************ + + public int getRightEncoder() { + return rightFrontTalon.getSelectedSensorPosition(0); + } + + public int getLeftEncoder() { + return leftFrontTalon.getSelectedSensorPosition(0); + } + + // speed is in inches per second + public double getRightSpeed() { + return rightFrontTalon.getSelectedSensorVelocity(0); + } + + public double getLeftSpeed() { + return leftFrontTalon.getSelectedSensorVelocity(0); + } + + // *************************** GYRO ******************************************* + + public double calculateHeadingError(double Target) { + double currentHeading = getHeading(); + double error = Target - currentHeading; + error = error % 360.0; + if (error > 180.0) { + error -= 360.0; + } + return error; + } + + public boolean getHeadingCorrectionMode() { + return m_useHeadingCorrection; + } + + public void setHeadingCorrectionMode(boolean useHeadingCorrection) { + m_useHeadingCorrection = useHeadingCorrection; + } + + private void resetAndEnableHeadingPID() { + if (Robot.shifter.getGear() == Shifter.HIGH_GEAR) { + m_HeadingPid.setP(HEADING_PID_P_FOR_HIGH_GEAR); + } else { + // low gear + m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR); + } + m_HeadingPid.reset(); + m_HeadingPid.enable(); + } + + static private final double STATIONARY = 0.1; + static private double m_prevLeftDistance = 0.0; + static private double m_prevRightDistance = 0.0; + + public boolean isStationary() { + double leftDistance = getLeftEncoder(); + double rightDistance = getRightEncoder(); + + double leftDelta = Math.abs(leftDistance - m_prevLeftDistance); + double rightDelta = Math.abs(rightDistance - m_prevRightDistance); + + m_prevLeftDistance = leftDistance; + m_prevRightDistance = rightDistance; + + return leftDelta < STATIONARY && rightDelta < STATIONARY; + } + + private int LoopCounter = 0; + + public void displayGyroInfo() { + SmartDashboard.putNumber("Robot Heading", Utils.twoDecimalPlaces(getHeading())); + SmartDashboard.putNumber("Robot Roll", Utils.twoDecimalPlaces(this.getRoll())); + SmartDashboard.putNumber("Robot Pitch", Utils.twoDecimalPlaces(this.getPitch())); + SmartDashboard.putNumber("Loop Counter", LoopCounter++); + } + + private double m_headingOffset = 0.0; + + public void setHeadingOffset(double arg_offset) { + m_headingOffset = arg_offset; + } + + public double getHeading() { + return Navx.getYaw() + m_headingOffset; + } + + // the Navx is installed sidways with reference to the front of the robot. + public double getRoll() { + return Navx.getPitch(); + } + + // the Navx is installed sidways with reference to the front of the robot. + public double getPitch() { + return Navx.getRoll(); + } + + public double getDesiredHeading() { + return m_desiredHeading; + } + + // ****************** SETTING POWER TO MOTORS ******************** + + public void resetNavXDisplacement() { + Navx.resetDisplacement(); + } + + public void stop() { + setMotorPower(0, 0); + } + + private void setMotorPower(double leftPower, double rightPower) { + if (rightPower > 1.0) { + rightPower = 1.0; + } + if (rightPower < -1.0) { + rightPower = -1.0; + } + + if (leftPower > 1.0) { + leftPower = 1.0; + } + if (leftPower < -1.0) { + leftPower = -1.0; + } + + if (m_closedLoopMode) { + rightFrontTalon.set(ControlMode.Velocity, rightPower * m_maxWheelSpeed); + leftFrontTalon.set(ControlMode.Velocity, leftPower * m_maxWheelSpeed); + } else { + rightFrontTalon.set(ControlMode.PercentOutput, rightPower); + leftFrontTalon.set(ControlMode.PercentOutput, leftPower); + } + } + + PowerDistributionPanel pdp = new PowerDistributionPanel(); + + /** + * updateSdbPdp Update the Smart Dashboard with the Power Distribution Panel + * currents. + */ + public void updateSdbPdp() { + double lf; + double rf; + double lb; + double rb; + double fudgeFactor = 0.0; + + lf = pdp.getCurrent(RobotMap.DRIVE_FRONT_LEFT_PDP) - fudgeFactor; + rf = pdp.getCurrent(RobotMap.DRIVE_FRONT_RIGHT_PDP) - fudgeFactor; + lb = pdp.getCurrent(RobotMap.DRIVE_BACK_LEFT_PDP) - fudgeFactor; + rb = pdp.getCurrent(RobotMap.DRIVE_BACK_RIGHT_PDP) - fudgeFactor; + + SmartDashboard.putNumber("Left Front I", lf); + SmartDashboard.putNumber("Right Front I", rf); + SmartDashboard.putNumber("Left Back I", lb); + SmartDashboard.putNumber("Right Back I", rb); + } + + /* + * This method allows one to drive in "Tank Drive Mode". Tank drive mode uses + * the left side of the joystick to control the left side of the robot, whereas + * the right side of the joystick controls the right side of the robot. + */ + public void tankDrive(double leftSideThrottle, double rightSideThrottle) { + if (leftSideThrottle >= 0.0) { + leftSideThrottle = (leftSideThrottle * leftSideThrottle); // squaring inputs increases fine control + } else { + leftSideThrottle = -(leftSideThrottle * leftSideThrottle); // preserves the sign while squaring negative + // values + } + + if (rightSideThrottle >= 0.0) { + rightSideThrottle = (rightSideThrottle * rightSideThrottle); + } else { + rightSideThrottle = -(rightSideThrottle * rightSideThrottle); + } + + setMotorPower(leftSideThrottle, rightSideThrottle); + } + + public void setAutoAlignTrue() { + autoAlign = true; + // reset the PID controller loop for steering now that we are auto-aligning + resetAndEnableHeadingPID(); + Robot.lights.set(LedPatternFactory.autoAlignTrying); + } + + public void setAutoAlignFalse() { + autoAlign = false; + resetAndEnableHeadingPID(); + Robot.lights.set(LedPatternFactory.autoAlignGotIt); + } + + public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickTurn) { + double leftPower, rightPower; + double rotation = 0; + double adjustedSteeringX = rawSteeringX * throttle; + final double QUICK_TURN_GAIN = 0.75; // culver drive used 1.5 + final double STD_TURN_GAIN = 1.5; // the driver wants the non-quick turn turning a little more responsive. + + int throttleSign; + if (throttle >= 0.0) { + throttleSign = 1; + } else { + throttleSign = -1; + } + if (autoAlign) { + // DriverStation.reportWarning("Auto align was called in drive base", false); + + // Use the targeting code to get the desired heading + m_desiredHeading = Robot.targeting.desiredHeading(); + + // Use the heading PID to provide the rotation input + rotation = maintainHeading(); + + } else { + // not using camera targeting right now + + // check for if steering input is essentially zero + if ((-0.01 < rawSteeringX) && (rawSteeringX < 0.01)) { + // no turn being commanded, drive straight or stay still + m_iterationsSinceRotationCommanded++; + if ((-0.01 < throttle) && (throttle < 0.01)) { + // no motion commanded, stay still + m_iterationsSinceMovementCommanded++; + rotation = 0.0; + m_desiredHeading = getHeading(); // whenever motionless, set desired heading to current heading + // reset the PID controller loop now that we have a new desired heading + resetAndEnableHeadingPID(); + } else { + // driving straight + if ((m_iterationsSinceRotationCommanded == LOOPS_GYRO_DELAY) + || (m_iterationsSinceMovementCommanded >= LOOPS_GYRO_DELAY)) { + // exactly LOOPS_GYRO_DELAY iterations with no commanded turn, + // or haven't had movement commanded for longer than LOOPS_GYRO_DELAY, + // so we want to take steps to preserve our current heading hereafter + + // get current heading as desired heading + m_desiredHeading = getHeading(); + + // reset the PID controller loop now that we have a new desired heading + resetAndEnableHeadingPID(); + rotation = 0.0; + } else if (m_iterationsSinceRotationCommanded < LOOPS_GYRO_DELAY) { + // not long enough since we were last turning, + // just drive straight without special heading maintenance + rotation = 0.0; + } else if (m_iterationsSinceRotationCommanded > LOOPS_GYRO_DELAY) { + // after more then LOOPS_GYRO_DELAY iterations since commanded turn, + // maintain the target heading + rotation = maintainHeading(); + } + m_iterationsSinceMovementCommanded = 0; + } + } else { + // commanding a turn, reset iterationsSinceRotationCommanded + m_iterationsSinceRotationCommanded = 0; + m_iterationsSinceMovementCommanded = 0; + + if (quickTurn) { + // want a high-rate turn (also allows "spin" behavior) + rotation = rawSteeringX * throttleSign * QUICK_TURN_GAIN; + } else { + // want a standard rate turn (scaled by the throttle) + rotation = adjustedSteeringX * STD_TURN_GAIN; // set the turn to the throttle-adjusted steering + // input + + } + } + } + // power to each wheel is a combination of the throttle and rotation + leftPower = throttle + rotation; + rightPower = throttle - rotation; + setMotorPower(leftPower, rightPower); + + } + + public int stage = 0; + public final double DELAY = 4; + public double timerStart = Timer.getFPGATimestamp(); + + public boolean selfTest() { + return false; + } + + /** + * The headings are from -180 to 180. The CurrentHeading is the current robot + * orientation. The TargetHeading is where we want the robot to face. + * + * e.g. Current = 0, Target = 10, error = 10 Current = 180, Target = -170, error + * = 10 (we need to turn 10 deg Clockwise to get to -170) Current = -90, Target + * = 180, error = -90 (we need to turn 90 deg Counter-Clockwise to get to 180) + * Current = 10, target = -10, error = -20 (we need to turn Counterclockwise -20 + * deg) + * + * @param CurrentHeading + * @param TargetHeading + * @return + */ + private double maintainHeading() { + double headingCorrection = 0.0; + + // below line is essential to let the Heading PID know the current heading error + m_HeadingError.m_Error = m_desiredHeading - getHeading(); + + if (m_useHeadingCorrection) { + headingCorrection = -m_HeadingCorrection.HeadingCorrection; + } else { + headingCorrection = 0.0; + } + + // check for major heading changes and take action to prevent + // integral windup if there is a major heading error + if (Math.abs(m_HeadingError.m_Error) > 10.0) { + if (!m_HeadingPidPreventWindup) { + m_HeadingPid.setI(0.0); + resetAndEnableHeadingPID(); + m_HeadingPidPreventWindup = true; + } + } else { + m_HeadingPidPreventWindup = false; + m_HeadingPid.setI(0.001); + } + + return headingCorrection; + } + + public void rotate(double RotateX) { + m_desiredHeading += RotateX; + if (m_desiredHeading > 180) { + m_desiredHeading -= 360; + } + if (m_desiredHeading < -180) { + m_desiredHeading += 360; + } + m_iterationsSinceRotationCommanded = LOOPS_GYRO_DELAY + 1; // hack so speedracerdrive maintains heading + m_iterationsSinceMovementCommanded = 0; + } + + public void rotateToHeading(double desiredHeading) { + m_desiredHeading = desiredHeading; + } + + // **********************************************DISPLAY**************************************************** + + public void updateSmartDashboard() { + displayGyroInfo(); + + // ***** KBS: Uncommenting below, as it takes a LONG time to get PDP values + // updateSdbPdp(); + + SmartDashboard.putNumber("Left Front Encoder Counts", leftFrontTalon.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Front Encoder Counts", rightFrontTalon.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Left Rear Encoder Counts", leftRearTalon.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Rear Encoder Counts", rightRearTalon.getSelectedSensorPosition(0)); + + // Note: getSpeed() returns ticks per 0.1 seconds + // SmartDashboard.putNumber("Left Encoder Speed", + // leftFrontTalon.getSelectedSensorVelocity(0)); + // SmartDashboard.putNumber("Right Encoder Speed", + // rightFrontTalon.getSelectedSensorVelocity(0)); + + // To convert ticks per 0.1 seconds into feet per second + // a - multiply be 10 (tenths of second per second) + // b - divide by 12 (1 foot per 12 inches) + // c - multiply by distance (in inches) per pulse + // SmartDashboard.putNumber("Left Speed (fps)", + // leftFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE); + // SmartDashboard.putNumber("Right Speed (fps)", + // rightFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * + // DISTANCE_PER_PULSE); + + SmartDashboard.putNumber("Left Talon Output Voltage", leftFrontTalon.getOutputVoltage()); + SmartDashboard.putNumber("Right Talon Output Voltage", rightFrontTalon.getOutputVoltage()); + + SmartDashboard.putBoolean("Closed Loop Mode", m_closedLoopMode); + SmartDashboard.putBoolean("Speed Racer Drive Mode", m_speedRacerDriveMode); + + SmartDashboard.putBoolean("Heading Correction Mode", m_useHeadingCorrection); + SmartDashboard.putNumber("Heading Desired", m_desiredHeading); + SmartDashboard.putNumber("Heading Error", m_HeadingError.m_Error); + SmartDashboard.putNumber("Heading Correction", -m_HeadingCorrection.HeadingCorrection); + + SmartDashboard.putNumber("Joystick Drive Throttle", Robot.oi.driveThrottle()); + SmartDashboard.putNumber("Joystick SteeringX", Robot.oi.steeringX()); + + // determine currentSpeed and display it + // double currentSpeed = getLeftSpeed() > getRightSpeed() ? getLeftSpeed() : + // getRightSpeed(); + + // display current speed to driver + // SmartDashboard.putNumber("Current Speed", currentSpeed); + } + + private static final double CAMERA_LAG = 0.150; // was .200; changing to .150 at CMP + + public void updateHistory() { + double now = Timer.getFPGATimestamp(); + headingHistory.add(now, getHeading()); + } + + public double getHeadingForCapturedImage() { + double now = Timer.getFPGATimestamp(); + double indexTime = now - CAMERA_LAG; + return headingHistory.getAzForTime(indexTime); + } + + /** + * Start a distance travel + */ + public void saveInitialWheelDistance() { + m_initialWheelDistance = (getLeftEncoder() + getRightEncoder()) / 2; + } + + /** + * Calculate the distance travelled. Return the second shortest distance. If a + * wheel is floating, it will have a larger value - ignore it. If a wheel is + * stuck, it will have a small value + * + * @return + */ + public double getWheelDistance() { + double dist = (getLeftEncoder() + getRightEncoder()) / 2; + return dist - m_initialWheelDistance; + } + + // NOTE the difference between rotateToHeading(...) and goToHeading(...) + public void setDesiredHeading(double desiredHeading) { + m_desiredHeading = desiredHeading; + m_iterationsSinceRotationCommanded = LOOPS_GYRO_DELAY + 1; + m_iterationsSinceMovementCommanded = 0; + + // reset the heading control loop for the new heading + resetAndEnableHeadingPID(); + } + +} diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java new file mode 100644 index 0000000..c58fae8 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java @@ -0,0 +1,19 @@ +package org.mayheminc.robot2019.subsystems; + +import edu.wpi.first.wpilibj.PIDOutput; + +/** + * + * @author user This class holds the correction that is calculated by the PID + * Controller. + */ +public class PIDHeadingCorrection implements PIDOutput { + + public double HeadingCorrection = 0.0; + + @Override + public void pidWrite(double output) { + HeadingCorrection = output; + } + +} diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java new file mode 100644 index 0000000..c19443f --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java @@ -0,0 +1,28 @@ +package org.mayheminc.robot2019.subsystems; + +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +/** + * + * @author user This is a class to hold the Heading error of the drive. + */ +public class PIDHeadingError implements PIDSource { + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + + } + + @Override + public PIDSourceType getPIDSourceType() { + return PIDSourceType.kDisplacement; + } + + @Override + public double pidGet() { + return m_Error; + } + + public double m_Error = 0.0; +} diff --git a/2020-Robot/vendordeps/navx_frc.json b/2020-Robot/vendordeps/navx_frc.json new file mode 100644 index 0000000..82810d3 --- /dev/null +++ b/2020-Robot/vendordeps/navx_frc.json @@ -0,0 +1,34 @@ +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "3.1.400", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2020/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "3.1.400" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "3.1.400", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian" + ] + } + ] +} \ No newline at end of file From 8d82e577e5f13d5bfad4a2fd4a8e1d6df81119a4 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Sat, 18 Jan 2020 14:33:03 -0500 Subject: [PATCH 006/121] resyncing repository --- .gitignore | 138 +++++++++++++++ {2020-Robot/.vscode => .vscode}/launch.json | 0 {2020-Robot/.vscode => .vscode}/settings.json | 0 .../wpilib_preferences.json | 0 2020-Robot/.gitignore | 161 ------------------ 2020-Robot/build.gradle => build.gradle | 0 .../wrapper/gradle-wrapper.jar | Bin .../wrapper/gradle-wrapper.properties | 0 2020-Robot/gradlew => gradlew | 0 2020-Robot/gradlew.bat => gradlew.bat | 0 2020-Robot/settings.gradle => settings.gradle | 0 .../src => src}/main/deploy/example.txt | 0 .../src => src}/main/java/frc/robot/Main.java | 0 .../main/java/frc/robot/Robot.java | 0 .../frc/robot/commands/ExampleCommand.java | 0 .../robot/subsystems/ExampleSubsystem.java | 0 .../org/mayheminc/robot2020/Constants.java | 56 ++++++ .../mayheminc/robot2020/RobotContainer.java | 0 .../org/mayheminc/util/AndJoystickButton.java | 0 .../util/DisabledOnlyJoystickButton.java | 0 .../util/EnabledOnlyJoystickButton.java | 0 .../main/java/org/mayheminc/util/History.java | 0 .../mayheminc/util/JoystickAxisButton.java | 0 .../org/mayheminc/util/JoystickPOVButton.java | 0 .../org/mayheminc/util/MB1340Ultrasonic.java | 0 .../org/mayheminc/util/MayhemTalonSRX.java | 0 .../org/mayheminc/util/ObjectListener.java | 0 .../org/mayheminc/util/ObjectLocation.java | 0 .../java/org/mayheminc/util/PidCycle.java | 0 .../java/org/mayheminc/util/PidTuner.java | 0 .../org/mayheminc/util/PidTunerObject.java | 0 .../mayheminc/util/RangeFinder_GP2D120.java | 0 .../org/mayheminc/util/SchedulerManager.java | 0 .../main/java/org/mayheminc/util/Utils.java | 0 .../vendordeps => vendordeps}/Phoenix.json | 0 .../WPILibNewCommands.json | 0 36 files changed, 194 insertions(+), 161 deletions(-) rename {2020-Robot/.vscode => .vscode}/launch.json (100%) rename {2020-Robot/.vscode => .vscode}/settings.json (100%) rename {2020-Robot/.wpilib => .wpilib}/wpilib_preferences.json (100%) delete mode 100644 2020-Robot/.gitignore rename 2020-Robot/build.gradle => build.gradle (100%) rename {2020-Robot/gradle => gradle}/wrapper/gradle-wrapper.jar (100%) rename {2020-Robot/gradle => gradle}/wrapper/gradle-wrapper.properties (100%) rename 2020-Robot/gradlew => gradlew (100%) rename 2020-Robot/gradlew.bat => gradlew.bat (100%) rename 2020-Robot/settings.gradle => settings.gradle (100%) rename {2020-Robot/src => src}/main/deploy/example.txt (100%) rename {2020-Robot/src => src}/main/java/frc/robot/Main.java (100%) rename {2020-Robot/src => src}/main/java/frc/robot/Robot.java (100%) rename {2020-Robot/src => src}/main/java/frc/robot/commands/ExampleCommand.java (100%) rename {2020-Robot/src => src}/main/java/frc/robot/subsystems/ExampleSubsystem.java (100%) create mode 100644 src/main/java/org/mayheminc/robot2020/Constants.java rename {2020-Robot/src => src}/main/java/org/mayheminc/robot2020/RobotContainer.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/AndJoystickButton.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/History.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/JoystickAxisButton.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/JoystickPOVButton.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/MB1340Ultrasonic.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/MayhemTalonSRX.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/ObjectListener.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/ObjectLocation.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/PidCycle.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/PidTuner.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/PidTunerObject.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/RangeFinder_GP2D120.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/SchedulerManager.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/Utils.java (100%) rename {2020-Robot/vendordeps => vendordeps}/Phoenix.json (100%) rename {2020-Robot/vendordeps => vendordeps}/WPILibNewCommands.json (100%) diff --git a/.gitignore b/.gitignore index a1c2a23..983678a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,40 @@ +# 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 @@ -21,3 +58,104 @@ # 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/ +imgui.ini + + +# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode diff --git a/2020-Robot/.vscode/launch.json b/.vscode/launch.json similarity index 100% rename from 2020-Robot/.vscode/launch.json rename to .vscode/launch.json diff --git a/2020-Robot/.vscode/settings.json b/.vscode/settings.json similarity index 100% rename from 2020-Robot/.vscode/settings.json rename to .vscode/settings.json diff --git a/2020-Robot/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json similarity index 100% rename from 2020-Robot/.wpilib/wpilib_preferences.json rename to .wpilib/wpilib_preferences.json diff --git a/2020-Robot/.gitignore b/2020-Robot/.gitignore deleted file mode 100644 index 983678a..0000000 --- a/2020-Robot/.gitignore +++ /dev/null @@ -1,161 +0,0 @@ -# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode - -### C++ ### -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app - -### Java ### -# Compiled class file -*.class - -# Log file -*.log - -# BlueJ files -*.ctxt - -# Mobile Tools for Java (J2ME) -.mtj.tmp/ - -# Package Files # -*.jar -*.war -*.nar -*.ear -*.zip -*.tar.gz -*.rar - -# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml -hs_err_pid* - -### Linux ### -*~ - -# temporary files which can be created if a process still has a handle open of a deleted file -.fuse_hidden* - -# KDE directory preferences -.directory - -# Linux trash folder which might appear on any partition or disk -.Trash-* - -# .nfs files are created when an open file is removed but is still being accessed -.nfs* - -### macOS ### -# General -.DS_Store -.AppleDouble -.LSOverride - -# Icon must end with two \r -Icon - -# Thumbnails -._* - -# Files that might appear in the root of a volume -.DocumentRevisions-V100 -.fseventsd -.Spotlight-V100 -.TemporaryItems -.Trashes -.VolumeIcon.icns -.com.apple.timemachine.donotpresent - -# Directories potentially created on remote AFP share -.AppleDB -.AppleDesktop -Network Trash Folder -Temporary Items -.apdisk - -### VisualStudioCode ### -.vscode/* -!.vscode/settings.json -!.vscode/tasks.json -!.vscode/launch.json -!.vscode/extensions.json - -### Windows ### -# Windows thumbnail cache files -Thumbs.db -ehthumbs.db -ehthumbs_vista.db - -# Dump file -*.stackdump - -# Folder config file -[Dd]esktop.ini - -# Recycle Bin used on file shares -$RECYCLE.BIN/ - -# Windows Installer files -*.cab -*.msi -*.msix -*.msm -*.msp - -# Windows shortcuts -*.lnk - -### Gradle ### -.gradle -/build/ - -# Ignore Gradle GUI config -gradle-app.setting - -# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) -!gradle-wrapper.jar - -# Cache of project -.gradletasknamecache - -# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 -# gradle/wrapper/gradle-wrapper.properties - -# # VS Code Specific Java Settings -.classpath -.project -.settings/ -bin/ -imgui.ini - - -# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode diff --git a/2020-Robot/build.gradle b/build.gradle similarity index 100% rename from 2020-Robot/build.gradle rename to build.gradle diff --git a/2020-Robot/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar similarity index 100% rename from 2020-Robot/gradle/wrapper/gradle-wrapper.jar rename to gradle/wrapper/gradle-wrapper.jar diff --git a/2020-Robot/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties similarity index 100% rename from 2020-Robot/gradle/wrapper/gradle-wrapper.properties rename to gradle/wrapper/gradle-wrapper.properties diff --git a/2020-Robot/gradlew b/gradlew similarity index 100% rename from 2020-Robot/gradlew rename to gradlew diff --git a/2020-Robot/gradlew.bat b/gradlew.bat similarity index 100% rename from 2020-Robot/gradlew.bat rename to gradlew.bat diff --git a/2020-Robot/settings.gradle b/settings.gradle similarity index 100% rename from 2020-Robot/settings.gradle rename to settings.gradle diff --git a/2020-Robot/src/main/deploy/example.txt b/src/main/deploy/example.txt similarity index 100% rename from 2020-Robot/src/main/deploy/example.txt rename to src/main/deploy/example.txt diff --git a/2020-Robot/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java similarity index 100% rename from 2020-Robot/src/main/java/frc/robot/Main.java rename to src/main/java/frc/robot/Main.java diff --git a/2020-Robot/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java similarity index 100% rename from 2020-Robot/src/main/java/frc/robot/Robot.java rename to src/main/java/frc/robot/Robot.java diff --git a/2020-Robot/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java similarity index 100% rename from 2020-Robot/src/main/java/frc/robot/commands/ExampleCommand.java rename to src/main/java/frc/robot/commands/ExampleCommand.java diff --git a/2020-Robot/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java similarity index 100% rename from 2020-Robot/src/main/java/frc/robot/subsystems/ExampleSubsystem.java rename to src/main/java/frc/robot/subsystems/ExampleSubsystem.java diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java new file mode 100644 index 0000000..ebba44a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean constants. This class should not be used for any other + * purpose. All constants should be declared globally (i.e. public static). Do + * not put anything functional in this class. + * + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the constants are needed, to reduce verbosity. + */ +public final class Constants { + public final class GAMEPAD_AXIS { + public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; + public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; + public static final int GAMEPAD_F310_LEFT_TRIGGER = 2; + public static final int GAMEPAD_F310_RIGHT_TRIGGER = 3; + public static final int GAMEPAD_F310_RIGHT_X_AXIS = 4; + public static final int GAMEPAD_F310_RIGHT_Y_AXIS = 5; + } + + public final class GAMEPAD_BUTTION { + public static final int GAMEPAD_F310_A_BUTTON = 1; + public static final int GAMEPAD_F310_B_BUTTON = 2; + public static final int GAMEPAD_F310_X_BUTTON = 3; + public static final int GAMEPAD_F310_Y_BUTTON = 4; + public static final int GAMEPAD_F310_LEFT_BUTTON = 5; + public static final int GAMEPAD_F310_RIGHT_BUTTON = 6; + public static final int GAMEPAD_F310_BACK_BUTTON = 7; + public static final int GAMEPAD_F310_START_BUTTON = 8; + public static final int GAMEPAD_F310_LEFT_STICK_BUTTON = 9; + public static final int GAMEPAD_F310_RIGHT_STICK_BUTTON = 10; + } + + public final class OPERATOR_PAD_AXIS { + public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; + public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; + public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; + public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; + } + + public final class Joysticks { + public static final int DRIVER_GAMEPAD = 0; + public static final int DRIVER_JOYSTICK = 1; + public static final int OPERATOR_GAMEPAD = 2; + public static final int OPERATOR_JOYSTICK = 3; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java rename to src/main/java/org/mayheminc/robot2020/RobotContainer.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java b/src/main/java/org/mayheminc/util/AndJoystickButton.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java rename to src/main/java/org/mayheminc/util/AndJoystickButton.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java b/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java rename to src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java b/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java rename to src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/History.java b/src/main/java/org/mayheminc/util/History.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/History.java rename to src/main/java/org/mayheminc/util/History.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java b/src/main/java/org/mayheminc/util/JoystickAxisButton.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java rename to src/main/java/org/mayheminc/util/JoystickAxisButton.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java b/src/main/java/org/mayheminc/util/JoystickPOVButton.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java rename to src/main/java/org/mayheminc/util/JoystickPOVButton.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java b/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java rename to src/main/java/org/mayheminc/util/MB1340Ultrasonic.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java rename to src/main/java/org/mayheminc/util/MayhemTalonSRX.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java b/src/main/java/org/mayheminc/util/ObjectListener.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java rename to src/main/java/org/mayheminc/util/ObjectListener.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java b/src/main/java/org/mayheminc/util/ObjectLocation.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java rename to src/main/java/org/mayheminc/util/ObjectLocation.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java b/src/main/java/org/mayheminc/util/PidCycle.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java rename to src/main/java/org/mayheminc/util/PidCycle.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java b/src/main/java/org/mayheminc/util/PidTuner.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java rename to src/main/java/org/mayheminc/util/PidTuner.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java b/src/main/java/org/mayheminc/util/PidTunerObject.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java rename to src/main/java/org/mayheminc/util/PidTunerObject.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java b/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java rename to src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java b/src/main/java/org/mayheminc/util/SchedulerManager.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java rename to src/main/java/org/mayheminc/util/SchedulerManager.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/Utils.java b/src/main/java/org/mayheminc/util/Utils.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/Utils.java rename to src/main/java/org/mayheminc/util/Utils.java diff --git a/2020-Robot/vendordeps/Phoenix.json b/vendordeps/Phoenix.json similarity index 100% rename from 2020-Robot/vendordeps/Phoenix.json rename to vendordeps/Phoenix.json diff --git a/2020-Robot/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json similarity index 100% rename from 2020-Robot/vendordeps/WPILibNewCommands.json rename to vendordeps/WPILibNewCommands.json From 22ffd9bd1645907ab4e1966182ba3055b246c335 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 18 Jan 2020 14:43:09 -0500 Subject: [PATCH 007/121] Adding the Util folder. Also adding some supporting classes for the Drive. --- 2020-Robot/build/tmp/jar/MANIFEST.MF | 3 + .../org/mayheminc/robot2020/Constants.java | 7 + .../mayheminc/robot2020/subsystems/Drive.java | 45 +-- .../subsystems/PIDHeadingCorrection.java | 2 +- .../robot2020/subsystems/PIDHeadingError.java | 2 +- .../org/mayheminc/util/AndJoystickButton.java | 23 ++ .../util/DisabledOnlyJoystickButton.java | 26 ++ .../util/EnabledOnlyJoystickButton.java | 26 ++ .../org/mayheminc/util/EventServer/Event.java | 6 + .../util/EventServer/EventServer.java | 52 ++++ .../util/EventServer/OneTimeEvent.java | 18 ++ .../mayheminc/util/EventServer/TCPServer.java | 65 +++++ .../util/EventServer/TMinusEvent.java | 24 ++ .../main/java/org/mayheminc/util/History.java | 59 ++++ .../mayheminc/util/JoystickAxisButton.java | 94 +++++++ .../org/mayheminc/util/JoystickPOVButton.java | 34 +++ .../org/mayheminc/util/MB1340Ultrasonic.java | 31 +++ .../org/mayheminc/util/MayhemTalonSRX.java | 153 ++++++++++ .../org/mayheminc/util/ObjectListener.java | 159 +++++++++++ .../org/mayheminc/util/ObjectLocation.java | 58 ++++ .../java/org/mayheminc/util/PidCycle.java | 11 + .../java/org/mayheminc/util/PidTuner.java | 174 ++++++++++++ .../org/mayheminc/util/PidTunerObject.java | 13 + .../mayheminc/util/RangeFinder_GP2D120.java | 60 ++++ .../org/mayheminc/util/SchedulerManager.java | 107 +++++++ .../main/java/org/mayheminc/util/Utils.java | 262 ++++++++++++++++++ 26 files changed, 1492 insertions(+), 22 deletions(-) create mode 100644 2020-Robot/build/tmp/jar/MANIFEST.MF create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/EventServer/Event.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/EventServer/EventServer.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/EventServer/TCPServer.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/History.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java create mode 100644 2020-Robot/src/main/java/org/mayheminc/util/Utils.java diff --git a/2020-Robot/build/tmp/jar/MANIFEST.MF b/2020-Robot/build/tmp/jar/MANIFEST.MF new file mode 100644 index 0000000..5c77096 --- /dev/null +++ b/2020-Robot/build/tmp/jar/MANIFEST.MF @@ -0,0 +1,3 @@ +Manifest-Version: 1.0 +Main-Class: frc.robot.Main + diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java index 8a0659a..7843674 100644 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java @@ -44,6 +44,13 @@ public final class Talon { public static final int CONTROL_PANEL_ROLLER = 16; } + public final class PDP { + public static final int DRIVE_RIGHT_A = 1; + public static final int DRIVE_RIGHT_B = 2; + public static final int DRIVE_LEFT_A = 3; + public static final int DRIVE_LEFT_B = 4; + } + public final class GAMEPAD_AXIS { public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 5b6e268..cef217c 100644 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -1,21 +1,24 @@ package org.mayheminc.robot2020.subsystems; import com.kauailabs.navx.frc.*; + +import org.mayheminc.robot2020.Constants; import org.mayheminc.util.History; import edu.wpi.first.wpilibj.*; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; -import edu.wpi.first.wpilibj.command.Subsystem; +//import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.mayheminc.robot2019.Robot; -import org.mayheminc.robot2019.RobotMap; +//import org.mayheminc.robot2020.Robot; +//import org.mayheminc.robot2019.RobotMap; import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.Utils; -public class Drive extends Subsystem { +public class Drive extends SubsystemBase { History headingHistory = new History(); @@ -31,10 +34,10 @@ public class Drive extends Subsystem { private static final int LOOPS_GYRO_DELAY = 10; // Talons - private final MayhemTalonSRX leftFrontTalon = new MayhemTalonSRX(RobotMap.LEFT_FRONT_TALON); - private final MayhemTalonSRX leftRearTalon = new MayhemTalonSRX(RobotMap.LEFT_REAR_TALON); - private final MayhemTalonSRX rightFrontTalon = new MayhemTalonSRX(RobotMap.RIGHT_FRONT_TALON); - private final MayhemTalonSRX rightRearTalon = new MayhemTalonSRX(RobotMap.RIGHT_REAR_TALON); + private final MayhemTalonSRX leftFrontTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_LEFT_A); + private final MayhemTalonSRX leftRearTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_LEFT_B); + private final MayhemTalonSRX rightFrontTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_RIGHT_A); + private final MayhemTalonSRX rightRearTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_RIGHT_B); // Sensors private AHRS Navx; @@ -257,9 +260,10 @@ public void setHeadingCorrectionMode(boolean useHeadingCorrection) { } private void resetAndEnableHeadingPID() { - if (Robot.shifter.getGear() == Shifter.HIGH_GEAR) { - m_HeadingPid.setP(HEADING_PID_P_FOR_HIGH_GEAR); - } else { + // if (Robot.shifter.getGear() == Shifter.HIGH_GEAR) { + // m_HeadingPid.setP(HEADING_PID_P_FOR_HIGH_GEAR); + // } else + { // low gear m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR); } @@ -364,10 +368,10 @@ public void updateSdbPdp() { double rb; double fudgeFactor = 0.0; - lf = pdp.getCurrent(RobotMap.DRIVE_FRONT_LEFT_PDP) - fudgeFactor; - rf = pdp.getCurrent(RobotMap.DRIVE_FRONT_RIGHT_PDP) - fudgeFactor; - lb = pdp.getCurrent(RobotMap.DRIVE_BACK_LEFT_PDP) - fudgeFactor; - rb = pdp.getCurrent(RobotMap.DRIVE_BACK_RIGHT_PDP) - fudgeFactor; + lf = pdp.getCurrent(Constants.PDP.DRIVE_LEFT_A) - fudgeFactor; + rf = pdp.getCurrent(Constants.PDP.DRIVE_LEFT_B) - fudgeFactor; + lb = pdp.getCurrent(Constants.PDP.DRIVE_RIGHT_A) - fudgeFactor; + rb = pdp.getCurrent(Constants.PDP.DRIVE_RIGHT_B) - fudgeFactor; SmartDashboard.putNumber("Left Front I", lf); SmartDashboard.putNumber("Right Front I", rf); @@ -401,13 +405,13 @@ public void setAutoAlignTrue() { autoAlign = true; // reset the PID controller loop for steering now that we are auto-aligning resetAndEnableHeadingPID(); - Robot.lights.set(LedPatternFactory.autoAlignTrying); + // Robot.lights.set(LedPatternFactory.autoAlignTrying); } public void setAutoAlignFalse() { autoAlign = false; resetAndEnableHeadingPID(); - Robot.lights.set(LedPatternFactory.autoAlignGotIt); + // Robot.lights.set(LedPatternFactory.autoAlignGotIt); } public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickTurn) { @@ -427,7 +431,7 @@ public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickT // DriverStation.reportWarning("Auto align was called in drive base", false); // Use the targeting code to get the desired heading - m_desiredHeading = Robot.targeting.desiredHeading(); + // m_desiredHeading = Robot.targeting.desiredHeading(); // Use the heading PID to provide the rotation input rotation = maintainHeading(); @@ -600,8 +604,9 @@ public void updateSmartDashboard() { SmartDashboard.putNumber("Heading Error", m_HeadingError.m_Error); SmartDashboard.putNumber("Heading Correction", -m_HeadingCorrection.HeadingCorrection); - SmartDashboard.putNumber("Joystick Drive Throttle", Robot.oi.driveThrottle()); - SmartDashboard.putNumber("Joystick SteeringX", Robot.oi.steeringX()); + // SmartDashboard.putNumber("Joystick Drive Throttle", + // Robot.oi.driveThrottle()); + // SmartDashboard.putNumber("Joystick SteeringX", Robot.oi.steeringX()); // determine currentSpeed and display it // double currentSpeed = getLeftSpeed() > getRightSpeed() ? getLeftSpeed() : diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java index c58fae8..809cec7 100644 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java @@ -1,4 +1,4 @@ -package org.mayheminc.robot2019.subsystems; +package org.mayheminc.robot2020.subsystems; import edu.wpi.first.wpilibj.PIDOutput; diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java index c19443f..7d1bf02 100644 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java +++ b/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java @@ -1,4 +1,4 @@ -package org.mayheminc.robot2019.subsystems; +package org.mayheminc.robot2020.subsystems; import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.PIDSourceType; diff --git a/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java new file mode 100644 index 0000000..8a7d92c --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java @@ -0,0 +1,23 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.buttons.Button; + +public class AndJoystickButton extends Button { + GenericHID joystick1; + GenericHID joystick2; + int buttonNumber1; + int buttonNumber2; + + public AndJoystickButton(GenericHID joystick1, int buttonNumber1, GenericHID joystick2, int buttonNumber2) { + this.joystick1 = joystick1; + this.buttonNumber1 = buttonNumber1; + this.joystick2 = joystick2; + this.buttonNumber2 = buttonNumber2; + } + + public boolean get() { + return joystick1.getRawButton(buttonNumber1) && joystick2.getRawButton(buttonNumber2); + } + +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java new file mode 100644 index 0000000..0d7329e --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java @@ -0,0 +1,26 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.buttons.Button; + +/** + * + * @author Team1519 + */ +public class DisabledOnlyJoystickButton extends Button { + + private GenericHID joystick; + private int buttonNumber; + private DriverStation ds; + + public DisabledOnlyJoystickButton(GenericHID joystick, int buttonNumber) { + this.joystick = joystick; + this.buttonNumber = buttonNumber; + ds = DriverStation.getInstance(); + } + + public boolean get() { + return joystick.getRawButton(buttonNumber) && ds.isDisabled(); + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java new file mode 100644 index 0000000..2b21309 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java @@ -0,0 +1,26 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.buttons.Button; + +/** + * + * @author Team1519 + */ +public class EnabledOnlyJoystickButton extends Button { + + private GenericHID joystick; + private int buttonNumber; + private DriverStation ds; + + public EnabledOnlyJoystickButton(GenericHID joystick, int buttonNumber) { + this.joystick = joystick; + this.buttonNumber = buttonNumber; + ds = DriverStation.getInstance(); + } + + public boolean get() { + return joystick.getRawButton(buttonNumber) && ds.isEnabled(); + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/Event.java b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/Event.java new file mode 100644 index 0000000..c968a2d --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/Event.java @@ -0,0 +1,6 @@ +package org.mayheminc.util.EventServer; + +public abstract class Event { + public abstract String Execute(); + +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/EventServer.java b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/EventServer.java new file mode 100644 index 0000000..cc77195 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/EventServer.java @@ -0,0 +1,52 @@ +package org.mayheminc.util.EventServer; + +import java.util.Vector; + +/** + * The Event Server has a TCP Server. Every 100 ms it loops through the events + * and lets them send messages to the TCP Server. + */ +public class EventServer extends Thread { + + Vector EventList = new Vector(); + TCPServer tcpServer = new TCPServer(); + + /** + * add an event object to the event list that will be polled every 100ms. + * + * @param E + */ + public void add(Event E) { + EventList.add(E); + } + + /** + * output a string to the TCP Server. This can be used by the events to signal + * thier events or single events can fire strings directly to the TCP Server + * + * @param str + */ + public void output(String str) { + tcpServer.add(str); + } + + /** + * Run the event server. Run the TCP Server. Loop through the events every + * 100ms. + */ + public void run() { + tcpServer.start(); + + while (true) { + try { + Thread.sleep(100); + + // execute each event + EventList.forEach((n) -> n.Execute()); + + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java new file mode 100644 index 0000000..f8b7e9c --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java @@ -0,0 +1,18 @@ +package org.mayheminc.util.EventServer; + +public abstract class OneTimeEvent extends Event { + boolean hasExecuted = false; + + public String Execute() { + if (hasExecuted) { + return ""; + } + String S = this.OneTimeExecute(); + if (S != "") { + hasExecuted = true; + } + return S; + } + + abstract public String OneTimeExecute(); +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/TCPServer.java b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/TCPServer.java new file mode 100644 index 0000000..b44ec9a --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/TCPServer.java @@ -0,0 +1,65 @@ +package org.mayheminc.util.EventServer; + +import java.io.*; +import java.net.*; +import java.util.concurrent.ArrayBlockingQueue; + +import edu.wpi.first.wpilibj.DriverStation; + +class TCPServer extends Thread { + + private static final int PORT = 5809; + + ArrayBlockingQueue buffer = new ArrayBlockingQueue<>(10); + + public static void main(String argv[]) throws Exception { + TCPServer server = new TCPServer(); + server.start(); + + Thread.sleep(5000); + server.add("bugs_answer.wav\n"); + + Thread.sleep(5000); + server.add("bugs_answer.wav\n"); + Thread.sleep(5000); + server.add("bugs_answer.wav\n"); + Thread.sleep(5000); + server.add("bugs_answer.wav\n"); + } + + public void add(String S) { + // add S to the buffer, but if we can't fail silently + buffer.offer(S); + } + + public void run() { + ServerSocket welcomeSocket; + try { + String wavfile; + + welcomeSocket = new ServerSocket(PORT); + + try { + while (true) { + Socket connectionSocket = welcomeSocket.accept(); + + System.out.println("Opening socket"); + DriverStation.reportError("Opening Socket for Sound", false); + + DataOutputStream outToClient = new DataOutputStream(connectionSocket.getOutputStream()); + + while (true) { + wavfile = buffer.take(); // take from the buffer. Wait if nothing is available + outToClient.writeBytes(wavfile + "\n"); + DriverStation.reportError("Sending Sound: " + wavfile, false); + } + } + } catch (Exception ex) { + } finally { + welcomeSocket.close(); + } + + } catch (Exception ex) { + } + } +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java new file mode 100644 index 0000000..22d07f2 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java @@ -0,0 +1,24 @@ +package org.mayheminc.util.EventServer; + +import edu.wpi.first.wpilibj.DriverStation; + +public class TMinusEvent extends OneTimeEvent { + String name; + double time; + final double MATCH_TIME_SEC = 150.0; + + public TMinusEvent(String S, int T) { + name = S; + time = T; + } + + public String OneTimeExecute() { + + // if the match time left is less than the time, return the name. + if (DriverStation.getInstance().getMatchTime() < time) { + return name; + } + return ""; + } + +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/History.java b/2020-Robot/src/main/java/org/mayheminc/util/History.java new file mode 100644 index 0000000..b91707f --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/History.java @@ -0,0 +1,59 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.DriverStation; + +public class History { + private static final int HISTORY_SIZE = 50; + + private double time[] = new double[HISTORY_SIZE]; + private double azimuth[] = new double[HISTORY_SIZE]; + private int index = 0; + + public History() { + // ensure there is at least one element in the history + add(-1.0, 0.0); // make a fictitious element at t=-1 seconds, with heading of 0.0 degrees + } + + public void add(double t, double az) { + + time[index] = t; + azimuth[index] = az; + + index++; + if (index >= HISTORY_SIZE) { + index = 0; + } + } + + public double getAzForTime(double t) { + double az = azimuth[index]; + int i = index - 1; + int count = 0; + + // if (t < 0) { + // DriverStation.reportError("Negative time in history", false); + // return 0.0; // no negative times. + // } + + while (i != index) { + if (i < 0) { + i = HISTORY_SIZE - 1; + } + + if (time[i] <= t) { + az = azimuth[i]; + break; + } + + i--; + count++; + if (count > HISTORY_SIZE) { + DriverStation.reportError("Looking too far back", false); + az = azimuth[index]; + break; + } + } + + return az; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java b/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java new file mode 100644 index 0000000..0b12f7c --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java @@ -0,0 +1,94 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.Button; + +/** + * + * @author Team1519 + */ +public class JoystickAxisButton extends Button { + public static final int BOTH_WAYS = 1; + public static final int POSITIVE_ONLY = 2; + public static final int NEGATIVE_ONLY = 3; + + private static final double AXIS_THRESHOLD = 0.2; + + private Joystick joystick; + private Joystick.AxisType axis; + private int axisInt; + private int direction; + + private double getAxis(Joystick.AxisType axis) { + switch (axis) { + case kX: + return joystick.getX(); + case kY: + return joystick.getY(); + case kZ: + return joystick.getZ(); + case kThrottle: + return joystick.getThrottle(); + case kTwist: + return joystick.getTwist(); + default: + // Unreachable + return 0.0; + } + } + + public JoystickAxisButton(Joystick stick, Joystick.AxisType axis) { + this(stick, axis, BOTH_WAYS); + } + + public JoystickAxisButton(Joystick stick, Joystick.AxisType axis, int direction) { + joystick = stick; + this.axis = axis; + this.direction = direction; + } + + public JoystickAxisButton(Joystick stick, int axis) { + this(stick, axis, BOTH_WAYS); + } + + public JoystickAxisButton(Joystick stick, int axis, int direction) { + joystick = stick; + this.axis = null; + axisInt = axis; + this.direction = direction; + } + + public boolean get() { + switch (direction) { + case BOTH_WAYS: + if (axis != null) { + return Math.abs(getAxis(axis)) > AXIS_THRESHOLD; + } + else { + return Math.abs(joystick.getRawAxis(axisInt)) > AXIS_THRESHOLD; + } + + case POSITIVE_ONLY: + if (axis != null) { + return getAxis(axis) > AXIS_THRESHOLD; + } + else { + return joystick.getRawAxis(axisInt) > AXIS_THRESHOLD; + } + + case NEGATIVE_ONLY: + if (axis != null) { + return getAxis(axis) < -AXIS_THRESHOLD; + } + else { + return joystick.getRawAxis(axisInt) < -AXIS_THRESHOLD; + } + } + + return false; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java b/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java new file mode 100644 index 0000000..a4bef2e --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java @@ -0,0 +1,34 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.Button; + +/** + * @author Team1519 + */ +public class JoystickPOVButton extends Button { + public static final int NORTH = 0; + public static final int NORTHEAST = 45; + public static final int EAST = 90; + public static final int SOUTHEAST = 135; + public static final int SOUTH = 180; + public static final int SOUTHWEST = 225; + public static final int WEST = 270; + public static final int NORTHWEST = 315; + + private Joystick joystick; + private int desiredPOV; + + public JoystickPOVButton(Joystick stick, int newDesiredPOV) { + joystick = stick; + desiredPOV = newDesiredPOV; + } + + public boolean get() { + return (joystick.getPOV() == desiredPOV); + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java b/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java new file mode 100644 index 0000000..76442a5 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java @@ -0,0 +1,31 @@ +package org.mayheminc.util; +import edu.wpi.first.wpilibj.AnalogInput; + +/* + * The Ultrasonic sensor being used is an MB1340. + * The conversion factor being used is (5 volts) / (1024 per cm). + */ + +public class MB1340Ultrasonic { + AnalogInput analogUltrasonic; + final static double voltage = 5.0; + final static double maxADCBins = 1024; + + public MB1340Ultrasonic(int analogPort) { + analogUltrasonic = new AnalogInput(analogPort); + } + + /** + * Get the Distance in cm. + * Full scale is 1024 cm. The minimum is 25cm. + * @return + */ + public double getDistance() { + return (analogUltrasonic.getAverageVoltage() / voltage) * maxADCBins; + } + + public double getDistanceInInches() { + return getDistance() / 2.54; + } + +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java new file mode 100644 index 0000000..d605650 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -0,0 +1,153 @@ +package org.mayheminc.util; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.*; + +public class MayhemTalonSRX extends TalonSRX { + + ControlMode controlMode; + double p; + double i; + double d; + double f; + + public MayhemTalonSRX(int deviceNumber) { + super(deviceNumber); + + this.configNominalOutputForward(0.0, 0); + this.configNominalOutputReverse(0.0, 0); + this.configPeakOutputForward(1.0, 0); + this.configPeakOutputReverse(-1.0, 0); + + this.setNeutralMode(NeutralMode.Coast); + +// this.configContinuousCurrentLimit(0, 0); +// this.configPeakCurrentLimit(0, 0); +// this.configPeakCurrentDuration(0, 0); +// this.configForwardLimitSwitchSource(RemoteLimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled, 0, 0); +// this.configForwardSoftLimitEnable(false, 0); + + // copied from CTRE Example: https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java/Current%20Limit/src/org/usfirst/frc/team217/robot/Robot.java#L37 +// this.configPeakCurrentLimit(80, 10); +// this.configPeakCurrentDuration(60000, 10); /* this is a necessary call to avoid errata. */ +// this.configContinuousCurrentLimit(40, 10); +// this.enableCurrentLimit(true); /* honor initial setting */ + + } + + public ErrorCode config_kP(int slot, double value, int timeout) + { + p = value; + return super.config_kP(slot, value, timeout); + } + + public ErrorCode config_kI(int slot, double value, int timeout) + { + i = value; + return super.config_kI(slot, value, timeout); + } + + public ErrorCode config_kD(int slot, double value, int timeout) + { + d = value; + return super.config_kD(slot, value, timeout); + } + public ErrorCode config_kF(int slot, double value, int timeout) + { + f = value; + return super.config_kF(slot, value, timeout); + } + + public double getP() {return p;} + public double getI() {return i;} + public double getD() {return d;} + public double getF() {return f;} + + public void changeControlMode(ControlMode mode) { + controlMode = mode; + } + + public void set(int deviceID) { + this.set(controlMode, deviceID); + } + + public void setFeedbackDevice(FeedbackDevice feedback) { + this.configSelectedFeedbackSensor(feedback, 0, 1000); + } + + public void reverseSensor(boolean b) { + + } + + public void configNominalOutputVoltage(float f, float g) { + this.configNominalOutputForward(f/12.0, 1000); + this.configNominalOutputReverse(g/12.0, 1000); + } + + public void configPeakOutputVoltage(double d, double e) { + this.configPeakOutputForward(d/12.0, 1000); + this.configPeakOutputReverse(e/12.0, 1000); + + } + + public void setPID(double wheelP, double wheelI, double wheelD, double wheelF, int i, double m_voltageRampRate, + int j) { + this.config_kP(pidProfile, wheelP , 1000); + this.config_kI(pidProfile, wheelI , 1000); + this.config_kD(pidProfile, wheelD , 1000); + this.config_kF(pidProfile, wheelF , 1000); + } + + public double getSetpoint() { + return 0; + } + + public double getError() { + return this.getClosedLoopError(0); + } + + public float getOutputVoltage() { + return (float) this.getMotorOutputVoltage(); + } + + int pidProfile; + public void setProfile(int pidSlot) { + pidProfile = pidSlot; + } + + public void setPID(double pDown, double iDown, double dDown) { + setPID(pDown, iDown, dDown, 0.0, 0, 0.0, 0); + } + + public void setVoltageRampRate(double d) { + // Need to convert volts per second to time + this.configClosedloopRamp(0, 0); + } + + public void enableControl() { + + } + + public void setPosition(int zeroPositionCount) { + this.setSelectedSensorPosition(zeroPositionCount, 0, 1000); + } + + public int getPosition() { + return this.getSelectedSensorPosition(0); + } + + public double getSpeed() { + return this.getSelectedSensorVelocity(0); + } + + public void setEncPosition(int i) { + setPosition(i); + } + + public double get() { + return this.getOutputCurrent(); + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java b/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java new file mode 100644 index 0000000..5cae99b --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java @@ -0,0 +1,159 @@ +package org.mayheminc.util; + +import java.util.*; +import java.io.*; +import java.net.*; +import java.nio.*; + +public class ObjectListener extends Thread { + protected static final int MAX_OBJECTS_PER_FRAME = 20; + protected static final int MAYHEM_MAGIC = 0x1519B0B4; + protected static final int MAX_BUFFER = 1500; + protected static final int DEFAULT_PORT = 5810; + + private DatagramSocket socket; + private DatagramPacket packet; + private ByteBuffer buffer; + private int lastFrame = 0; + private ArrayList objList; + private Callback callback = null; + + public interface Callback { + public void objectListenerCallback(int frame, ArrayList objList); + } + + public ObjectListener() throws SocketException { + this(DEFAULT_PORT); + } + + public ObjectListener(int port) throws SocketException { + super("ObjectListener-" + port); + + socket = new DatagramSocket(port); + + byte[] byteBuffer = new byte[MAX_BUFFER]; + packet = new DatagramPacket(byteBuffer, byteBuffer.length); + buffer = ByteBuffer.wrap(byteBuffer); + + objList = new ArrayList(); + } + + public int getLastFrame() { + return lastFrame; + } + + public List getObjectList() { + return objList; + } + + public void setCallback(Callback callback) { + this.callback = callback; + } + + public void run() { + String name = super.getName(); + long lastTimestamp = 0; + + while (true) { + // Receive new datagram + try { + socket.receive(packet); + buffer.rewind(); + } catch (IOException e) { + System.err.println(super.getName() + " encountered an error"); + e.printStackTrace(); + System.err.println(super.getName() + " aborting"); + break; + } + + // Abort if told to do so + if (Thread.interrupted()) + break; + + // Validate packet + int magic = buffer.getInt(); + if (magic != MAYHEM_MAGIC) { + System.err.println(name + ": invalid packet received (magic == 0x" + Integer.toHexString(magic) + ")"); + continue; + } + + // Get information about the update + int frame = buffer.getInt(); + long timestamp = buffer.getLong(); + + // Check for out-of-date data + if (timestamp <= lastTimestamp) { + System.err.println(name + ": timestamp for new frame #" + frame + " (" + timestamp + ") is not newer than that for previous frame #" + lastFrame + " (" + lastTimestamp + "); rejecting out-of-date data"); + lastTimestamp = timestamp; + continue; + } + if (frame <= lastFrame) { + System.err.println(name + ": frame #" + frame + " is earlier than existing frame #" + lastFrame + "; did object detection service restart?"); + } + + // Get list of all objects involved + ArrayList objList = new ArrayList(); + for (int i = 0; i < MAX_OBJECTS_PER_FRAME; i++) { + // Pull the next object from the buffer + ObjectLocation loc = new ObjectLocation(buffer); + + // As soon as one object is none, so are the rest + if (loc.type == ObjectLocation.ObjectTypes.OBJ_NONE) + break; + + // Add object to our list + objList.add(loc); + } + + // Update the list of objects + this.objList = objList; + lastFrame = frame; + lastTimestamp = timestamp; + + // Invoke callback, if applicable + if (callback != null) { + callback.objectListenerCallback(frame, objList); + } + } + + // Clean up + socket.close(); + } + + // Sample implementation for testing and demonstration purposes + public static void main(String[] args) { + ObjectListener listener; + Callback callback = new ObjectListener.Callback() { + public void objectListenerCallback(int frame, ArrayList objList) { + System.out.println("Received notification about objects in frame #" + frame); + for (ObjectLocation loc: objList) { + System.out.println(" " + loc); + } + } + }; + + // Create the listener + try { + listener = new ObjectListener(); + } catch (SocketException e) { + e.printStackTrace(); + return; + } + + // Use the callback implementation + listener.setCallback(callback); + + // Begin listening + System.out.println("Starting object listener...\n"); + listener.start(); + + // Wait forever -- notifications will come from callback + while (true) { + try { + Thread.sleep(600); + } catch (InterruptedException e) { + break; + } + } + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java b/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java new file mode 100644 index 0000000..a903461 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java @@ -0,0 +1,58 @@ +package org.mayheminc.util; + +import java.nio.*; +import java.text.DecimalFormat; + +public class ObjectLocation { + public enum ObjectTypes { + OBJ_NONE, + OBJ_CUBE, + OBJ_SCALE_CENTER, + OBJ_SCALE_BLUE, + OBJ_SCALE_RED, + OBJ_SWITCH_RED, + OBJ_SWITCH_BLUE, + OBJ_PORTAL_RED, + OBJ_PORTAL_BLUE, + OBJ_EXCHANGE_RED, + OBJ_EXCHANGE_BLUE, + OBJ_BUMPERS_RED, + OBJ_BUMPERS_BLUE, + OBJ_BUMPERS_CLASS13, + OBJ_BUMPERS_CLASS14, + OBJ_BUMPERS_CLASS15, + OBJ_BUMPERS_CLASS16, + OBJ_BUMPERS_CLASS17, + OBJ_BUMPERS_CLASS18, + OBJ_BUMPERS_CLASS19, + OBJ_BUMPERS_CLASS20, + OBJ_EOL, + }; + + public ObjectTypes type; + public float x; + public float y; + public float width; + public float height; + public float probability; + + public ObjectLocation() { + type = ObjectTypes.OBJ_NONE; + x = y = width = height = probability = 0; + } + + public ObjectLocation(ByteBuffer buffer) { + type = ObjectTypes.values()[buffer.getInt()]; + x = (float)buffer.getInt() / Integer.MAX_VALUE; + y = (float)buffer.getInt() / Integer.MAX_VALUE; + width = (float)buffer.getInt() / Integer.MAX_VALUE; + height = (float)buffer.getInt() / Integer.MAX_VALUE; + probability = (float)buffer.getInt() / Integer.MAX_VALUE; + } + + public String toString() { + DecimalFormat df = new DecimalFormat("0.00"); + + return type.name() + "@" + df.format(x) + "x" + df.format(y) + "+" + df.format(width) + "x" + df.format(height) + "[" + df.format(probability * 100) + "%]"; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java b/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java new file mode 100644 index 0000000..4b9b719 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java @@ -0,0 +1,11 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.command.InstantCommand; + +public class PidCycle extends InstantCommand +{ + protected void initialize() { +// Robot.pidTuner.cyclePid(); + } + +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java b/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java new file mode 100644 index 0000000..1748831 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java @@ -0,0 +1,174 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * This is a class to help tune a PID. + * Another class that extends IPidTunerObject is passed in. + * This interface defines methods that are used to set the P-I-D values. + * This class gets joystick buttons that define the different options. + * - Button 1: cycle through P-I-D values + * - Button 2: increase the value. + * - Button 3: decrease the value. + * - Button 4: cycle through the amount the value changes: 10, 1, .1, .01, .001, .0001 + * This class needs to be called for updateSmartDashboard(). + * @author User + * + */ +public class PidTuner extends InstantCommand{ + Button m_PidCycle; + Button m_Inc; + Button m_Dec; + Button m_ValueCycle; + PidTunerObject m_pidObj; + + enum PidCycle {P, I, D, F}; + int m_cycle; + int m_position; + + // Remember the buttons and call this object when they are pressed. + // remember the PID Object so we can get/set the PID values. + public PidTuner(Button b1, Button b2, Button b3, Button b4, PidTunerObject obj) + { + setRunWhenDisabled(true); + + b1.whenPressed(this); + b2.whenPressed(this); + b3.whenPressed(this); + b4.whenPressed(this); + + m_PidCycle = b1; + m_ValueCycle = b2; + m_Inc = b3; + m_Dec = b4; + + m_pidObj = obj; + } + + // Run the 'instant command'. Determine which command was pressed. + protected void initialize() { + if( m_PidCycle.get()) + { + RunCycle(); + } + if( m_Inc.get()) + { + RunInc(); + } + if( m_Dec.get()) + { + RunDec(); + } + if( m_ValueCycle.get()) + { + RunValue(); + } + } + + // set the P, I, or D that we are changing + public void RunCycle() + { +// System.out.println("PID Tuner: RunCycle"); + m_cycle++; + m_cycle = m_cycle % 4; + } + + String getCycleStr() + { + String str[] = {"P", "I", "D", "F"}; + + return str[m_cycle]; + } + + // calculate the amount to increment, get the value, apply the amount, set the new value + public void RunInc() + { +// System.out.println("PID Tuner: RunInc"); + double amount = calculateAmount(); + + double value = getValue(); + value = value + amount; + setValue(value); + } + + // if m_position is 1, return 10, 0 returns 1, -1 returns 0.1, etc. + double calculateAmount() + { + double retval = Math.pow(10, m_position); + return retval; + } + + // based on the cycle, get the P, I, or D. + double getValue() + { + switch(m_cycle) + { + case 0: + return m_pidObj.getP(); + case 1: + return m_pidObj.getI(); + case 2: + return m_pidObj.getD(); + case 3: + return m_pidObj.getF(); + } + return 0.0; + } + + // based on the cycle, set the P, I, or D. + void setValue(double d) + { + switch(m_cycle) + { + case 0: + m_pidObj.setP(d); + break; + case 1: + m_pidObj.setI(d); + break; + case 2: + m_pidObj.setD(d); + break; + case 3: + m_pidObj.setF(d); + break; + } + } + + // calculate the amount to decrement, get the value, apply the amount, set the new value + public void RunDec() + { +// System.out.println("PID Tuner: RunDec"); + double amount = calculateAmount(); + + double value = getValue(); + value = value - amount; + setValue(value); + } + + // Change the amount we are decrementing or incrementing + public void RunValue() + { +// System.out.println("PID Tuner: RunValue"); + + m_position--; + if( m_position < -4) + { + m_position = 1; // 10.1234 + } + } + + public void updateSmartDashboard() + { + SmartDashboard.putNumber("PID Tuner P", m_pidObj.getP()); + SmartDashboard.putNumber("PID Tuner I", m_pidObj.getI()); + SmartDashboard.putNumber("PID Tuner D", m_pidObj.getD()); + SmartDashboard.putNumber("PID Tuner F", m_pidObj.getF()); + + SmartDashboard.putString("PID Tuner Cycle", getCycleStr()); + SmartDashboard.putNumber("PID Tuner Amount", calculateAmount()); + } + +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java b/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java new file mode 100644 index 0000000..d9a59d0 --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java @@ -0,0 +1,13 @@ +package org.mayheminc.util; + +public interface PidTunerObject { + + double getP(); + double getI(); + double getD(); + double getF(); + void setP(double d); + void setI(double d); + void setD(double d); + void setF(double d); +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java b/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java new file mode 100644 index 0000000..061fcbc --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java @@ -0,0 +1,60 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.AnalogInput; + +// http://media.digikey.com/pdf/Data%20Sheets/Sharp%20PDFs/GP2D120.pdf + +public class RangeFinder_GP2D120 { + + private AnalogInput m_channel; + private static final int HISTORY_BUFFER_SIZE = 5; + private double m_historyBuffer[]=new double[HISTORY_BUFFER_SIZE]; + private int m_historyBufferIndex; + private double m_filteredVoltage; + + public RangeFinder_GP2D120(int channel, int index) { + m_channel = new AnalogInput(channel); + m_filteredVoltage = 0.0; + } + + public void periodic() { + double min = m_historyBuffer[0]; + m_historyBuffer[m_historyBufferIndex]= m_channel.getVoltage(); //return channel.getVoltage(); + m_historyBufferIndex++; + + if (m_historyBufferIndex>=HISTORY_BUFFER_SIZE) { + m_historyBufferIndex=0; + } + + for (int i=1; i= OBJECT_IS_SEEN_VOLTAGE; + } + + // had been 0.80 prior to 31 Jan + private static final double OBJECT_IS_CLOSE_VOLTAGE = 0.80; + public boolean isObjectClose() { + return getFilteredVoltage() >= OBJECT_IS_CLOSE_VOLTAGE; + } + + // added on 11 Feb 2015 to allow "floating off tote" + private static final double OBJECT_IS_VERY_CLOSE_VOLTAGE = 1.60; + public boolean isObjectVeryClose() { + return getFilteredVoltage() >= OBJECT_IS_VERY_CLOSE_VOLTAGE; + } + +} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java b/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java new file mode 100644 index 0000000..26300ab --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java @@ -0,0 +1,107 @@ +/** + * Copyright (c) 2015-2019 Team 501 - The PowerKnights. All Rights Reserved. + * Open Source Software - May be modified and shared by FRC teams. The code must + * be accompanied by the Team 501 BSD license file in the root directory of the + * project. You may also obtain a copy of it from the following: + * http://www.opensource.org/licenses/bsd-license.php. + * + * See (Git) repository metadata for author and revision history for this file. + **/ + +package org.mayheminc.util; + +// import org.slf4j.Logger; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableType; +import edu.wpi.first.networktables.NetworkTableValue; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +// import riolog.RioLogger; + +/** + * + */ +public class SchedulerManager // implements ITelemetryProvider +{ + + /* Our classes logger */ + // @SuppressWarnings("unused") + // private static final Logger logger = + // RioLogger.getLogger(SchedulerManager.class.getName()); + + /** Singleton instance of class for all to use **/ + private static SchedulerManager ourInstance; + /** Name of our subsystem **/ + private final static String myName = "Running Commands";// TelemetryNames.Scheduler.name; + + /** + * Constructs instance of the subsystem. Assumed to be called before any usage + * of the subsystem; and verifies only called once. Allows controlled startup + * sequencing of the robot and all it's subsystems. + **/ + public static synchronized void constructInstance() { + // SmartDashboard.putBoolean(TelemetryNames.Scheduler.status, false); + + if (ourInstance != null) { + throw new IllegalStateException(myName + " Already Constructed"); + } + ourInstance = new SchedulerManager(); + + // SmartDashboard.putBoolean(TelemetryNames.Scheduler.status, true); + } + + /** + * Returns the singleton instance of the subsystem in the form of the + * Interface that is defined for it. If it hasn't been constructed yet, + * throws an IllegalStateException. + * + * @return singleton instance of subsystem + **/ + public static SchedulerManager getInstance() { + if (ourInstance == null) { + throw new IllegalStateException(myName + " Not Constructed Yet"); + } + return ourInstance; + } + + private final NetworkTable liveWindow; + private final NetworkTable ungrouped; + private final NetworkTable scheduler; + private final StringBuilder names; + + public SchedulerManager() { + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + liveWindow = inst.getTable("LiveWindow"); + ungrouped = liveWindow.getSubTable("Ungrouped"); + scheduler = ungrouped.getSubTable("Scheduler"); + + names = new StringBuilder(); + } + + // @Override + public void updateTelemetry() { + NetworkTableEntry namesEntry = scheduler.getEntry("Names"); + NetworkTableValue namesValue = namesEntry.getValue(); + if ((namesValue == null)) { + SmartDashboard.putString("Current Commands", "namesValue is null"); + return; + } + + if ((namesValue.getType() != NetworkTableType.kStringArray)) { + + SmartDashboard.putString("Current Commands", "namesValue is " + namesValue.getType()); + return; + } + + names.setLength(0); + for (String name : namesValue.getStringArray()) { + names.append(name).append(": "); + } + + SmartDashboard.putString("Current Commands", names.toString()); + } + +} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/Utils.java b/2020-Robot/src/main/java/org/mayheminc/util/Utils.java new file mode 100644 index 0000000..edcabda --- /dev/null +++ b/2020-Robot/src/main/java/org/mayheminc/util/Utils.java @@ -0,0 +1,262 @@ +/* + * Utility class with useful methods in it + */ +package org.mayheminc.util; + +/** + * + * @author Team1519 + */ +public class Utils { + + public static double oneDecimalPlace(double d) { + return ((double) ((int) (d * 10))) / 10; + } + + public static double twoDecimalPlaces(double d) { + return ((double) ((int) (d * 100))) / 100; + } + + public static double threeDecimalPlaces(double d) { + return ((double) ((int) (d * 1000))) / 1000; + } + + public static double fourDecimalPlaces(double d) { + return ((double) ((int) (d * 10000))) / 10000; + } + + /** + * Returns the arc tangent of an angle, in the range of + * -Math.PI/2 through + * Math.PI/2. Special cases:

  • If the argument is + * NaN, then the result is + * NaN.
  • If the argument is zero, then the result is a zero + * with the same sign as the argument.

A result must be within 1 + * ulp of the correctly rounded result. Results must be semi-monotonic. + * + * @param a - the value whose arc tangent is to be returned. + * @return the arc tangent of the argument. + */ + public static double atan(double a) { + // Special cases. + if (Double.isNaN(a)) { + return Double.NaN; + } + + if (a == 0.0) { + return a; + } + + // Compute the arc tangent. + boolean negative = false; + boolean greaterThanOne = false; + int i = 0; + + if (a < 0.0) { + a = -a; + negative = true; + } + + if (a > 1.0) { + a = 1.0 / a; + greaterThanOne = true; + } + + double t; + + for (; a > PIover12; a *= t) { + i++; + t = a + ATAN_CONSTANT; + t = 1.0 / t; + a *= ATAN_CONSTANT; + a--; + } + + double aSquared = a * a; + + double arcTangent = aSquared + 1.4087812; + arcTangent = 0.55913709 / arcTangent; + arcTangent += 0.60310578999999997; + arcTangent -= 0.051604539999999997 * aSquared; + arcTangent *= a; + + for (; i > 0; i--) { + arcTangent += PIover6; + } + + if (greaterThanOne) { + arcTangent = PIover2 - arcTangent; + } + + if (negative) { + arcTangent = -arcTangent; + } + + return arcTangent; + } + /** + * Constant for PI divided by 2. + */ + public static final double PIover2 = Math.PI / 2; + /** + * Constant for PI divided by 4. + */ + public static final double PIover4 = Math.PI / 4; + /** + * Constant for PI divided by 6. + */ + public static final double PIover6 = Math.PI / 6; + /** + * Constant for PI divided by 12. + */ + public static final double PIover12 = Math.PI / 12; + /** + * Constant used in the + * atan calculation. + */ + private static final double ATAN_CONSTANT = 1.732050807569; + + /** + * Converts rectangular coordinates (x, y) to polar (r, theta). This + * method computes the phase theta by computing an arc tangent of y/x + * in the range of -pi to pi. Special cases:

  • If + * either argument is + * NaN, then the result is + * NaN.
  • If the first argument is positive zero and the + * second argument is positive, or the first argument is positive and finite + * and the second argument is positive infinity, then the result is positive + * zero.
  • If the first argument is negative zero and the second argument + * is positive, or the first argument is negative and finite and the second + * argument is positive infinity, then the result is negative zero.
  • If + * the first argument is positive zero and the second argument is negative, + * or the first argument is positive and finite and the second argument is + * negative infinity, then the result is the + * double value closest to pi.
  • If the first argument + * is negative zero and the second argument is negative, or the first + * argument is negative and finite and the second argument is negative + * infinity, then the result is the + * double value closest to -pi.
  • If the first + * argument is positive and the second argument is positive zero or negative + * zero, or the first argument is positive infinity and the second argument + * is finite, then the result is the + * double value closest to pi/2.
  • If the first + * argument is negative and the second argument is positive zero or negative + * zero, or the first argument is negative infinity and the second argument + * is finite, then the result is the + * double value closest to -pi/2.
  • If both arguments + * are positive infinity, then the result is the double value closest to + * pi/4.
  • If the first argument is positive infinity and the + * second argument is negative infinity, then the result is the double value + * closest to 3*pi/4.
  • If the first argument is negative infinity + * and the second argument is positive infinity, then the result is the + * double value closest to -pi/4.
  • If both arguments are negative + * infinity, then the result is the double value closest to -3*pi/4. + *

A result must be within 2 ulps of the correctly rounded result. + * Results must be semi-monotonic. + * + * @param y - the ordinate coordinate + * @param x - the abscissa coordinate + * @return the theta component of the point (r, theta) in + * polar coordinates that corresponds to the point (x, y) in Cartesian + * coordinates. + */ + public static double atan2(double y, double x) { + // Special cases. + if (Double.isNaN(y) || Double.isNaN(x)) { + return Double.NaN; + } else if (Double.isInfinite(y)) { + if (y > 0.0) // Positive infinity + { + if (Double.isInfinite(x)) { + if (x > 0.0) { + return PIover4; + } else { + return 3.0 * PIover4; + } + } else if (x != 0.0) { + return PIover2; + } + } else // Negative infinity + { + if (Double.isInfinite(x)) { + if (x > 0.0) { + return -PIover4; + } else { + return -3.0 * PIover4; + } + } else if (x != 0.0) { + return -PIover2; + } + } + } else if (y == 0.0) { + if (x > 0.0) { + return y; + } else if (x < 0.0) { + return Math.PI; + } + else { + return 0; + } + } else if (Double.isInfinite(x)) { + if (x > 0.0) // Positive infinity + { + if (y > 0.0) { + return 0.0; + } else if (y < 0.0) { + return -0.0; + } + } else // Negative infinity + { + if (y > 0.0) { + return Math.PI; + } else if (y < 0.0) { + return -Math.PI; + } + } + } else if (x == 0.0) { + if (y > 0.0) { + return PIover2; + } else if (y < 0.0) { + return -PIover2; + } + else { + // Should not get her with y test above + return 0; + } + } + + + // Implementation a simple version ported from a PASCAL implementation: + // http://everything2.com/index.pl?node_id=1008481 + + double arcTangent; + + // Use arctan() avoiding division by zero. + if (Math.abs(x) > Math.abs(y)) { + arcTangent = atan(y / x); + } else { + arcTangent = atan(x / y); // -PI/4 <= a <= PI/4 + + if (arcTangent < 0) { + arcTangent = -PIover2 - arcTangent; // a is negative, so we're adding + } else { + arcTangent = PIover2 - arcTangent; + } + } + + // Adjust result to be from [-PI, PI] + if (x < 0) { + if (y < 0) { + arcTangent = arcTangent - Math.PI; + } else { + arcTangent = arcTangent + Math.PI; + } + } + + return arcTangent; + } + + public static boolean isBetween(double value, double upper, double lower){ + return value < upper && value > lower; + } +} From 10254be644c72e5034bcef918617c899e222d5d3 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 18 Jan 2020 14:48:00 -0500 Subject: [PATCH 008/121] Some files were put in the wrong folder. --- .../mayheminc/robot2020/RobotContainer.java | 0 .../org/mayheminc/util/AndJoystickButton.java | 4 +- .../util/DisabledOnlyJoystickButton.java | 2 +- .../util/EnabledOnlyJoystickButton.java | 6 +- .../mayheminc/util/JoystickAxisButton.java | 93 +++++---- .../org/mayheminc/util/JoystickPOVButton.java | 12 +- .../java/org/mayheminc/util/PidCycle.java | 9 +- .../java/org/mayheminc/util/PidTuner.java | 180 +++++++++--------- 8 files changed, 146 insertions(+), 160 deletions(-) rename {src => 2020-Robot/src}/main/java/org/mayheminc/robot2020/RobotContainer.java (100%) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java similarity index 100% rename from src/main/java/org/mayheminc/robot2020/RobotContainer.java rename to 2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java index 8a7d92c..2c4da30 100644 --- a/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java +++ b/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java @@ -1,10 +1,10 @@ package org.mayheminc.util; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj2.command.button.*; public class AndJoystickButton extends Button { - GenericHID joystick1; + GenericHID joystick1; GenericHID joystick2; int buttonNumber1; int buttonNumber2; diff --git a/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java index 0d7329e..a76ab1c 100644 --- a/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java +++ b/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java @@ -2,7 +2,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj2.command.button.*; /** * diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java index 2b21309..9bd9d4a 100644 --- a/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java +++ b/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java @@ -2,14 +2,14 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj2.command.button.*; /** * * @author Team1519 */ public class EnabledOnlyJoystickButton extends Button { - + private GenericHID joystick; private int buttonNumber; private DriverStation ds; @@ -19,7 +19,7 @@ public EnabledOnlyJoystickButton(GenericHID joystick, int buttonNumber) { this.buttonNumber = buttonNumber; ds = DriverStation.getInstance(); } - + public boolean get() { return joystick.getRawButton(buttonNumber) && ds.isEnabled(); } diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java b/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java index 0b12f7c..0657aa4 100644 --- a/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java +++ b/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java @@ -5,7 +5,7 @@ package org.mayheminc.util; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj2.command.button.*; /** * @@ -15,80 +15,77 @@ public class JoystickAxisButton extends Button { public static final int BOTH_WAYS = 1; public static final int POSITIVE_ONLY = 2; public static final int NEGATIVE_ONLY = 3; - + private static final double AXIS_THRESHOLD = 0.2; - + private Joystick joystick; private Joystick.AxisType axis; private int axisInt; private int direction; - + private double getAxis(Joystick.AxisType axis) { - switch (axis) { - case kX: - return joystick.getX(); - case kY: - return joystick.getY(); - case kZ: - return joystick.getZ(); - case kThrottle: - return joystick.getThrottle(); - case kTwist: - return joystick.getTwist(); - default: - // Unreachable - return 0.0; - } + switch (axis) { + case kX: + return joystick.getX(); + case kY: + return joystick.getY(); + case kZ: + return joystick.getZ(); + case kThrottle: + return joystick.getThrottle(); + case kTwist: + return joystick.getTwist(); + default: + // Unreachable + return 0.0; + } } - + public JoystickAxisButton(Joystick stick, Joystick.AxisType axis) { this(stick, axis, BOTH_WAYS); } - + public JoystickAxisButton(Joystick stick, Joystick.AxisType axis, int direction) { joystick = stick; this.axis = axis; this.direction = direction; } - + public JoystickAxisButton(Joystick stick, int axis) { this(stick, axis, BOTH_WAYS); } - + public JoystickAxisButton(Joystick stick, int axis, int direction) { joystick = stick; this.axis = null; axisInt = axis; this.direction = direction; } - + public boolean get() { switch (direction) { - case BOTH_WAYS: - if (axis != null) { - return Math.abs(getAxis(axis)) > AXIS_THRESHOLD; - } - else { - return Math.abs(joystick.getRawAxis(axisInt)) > AXIS_THRESHOLD; - } - - case POSITIVE_ONLY: - if (axis != null) { - return getAxis(axis) > AXIS_THRESHOLD; - } - else { - return joystick.getRawAxis(axisInt) > AXIS_THRESHOLD; - } - - case NEGATIVE_ONLY: - if (axis != null) { - return getAxis(axis) < -AXIS_THRESHOLD; - } - else { - return joystick.getRawAxis(axisInt) < -AXIS_THRESHOLD; - } + case BOTH_WAYS: + if (axis != null) { + return Math.abs(getAxis(axis)) > AXIS_THRESHOLD; + } else { + return Math.abs(joystick.getRawAxis(axisInt)) > AXIS_THRESHOLD; + } + + case POSITIVE_ONLY: + if (axis != null) { + return getAxis(axis) > AXIS_THRESHOLD; + } else { + return joystick.getRawAxis(axisInt) > AXIS_THRESHOLD; + } + + case NEGATIVE_ONLY: + if (axis != null) { + return getAxis(axis) < -AXIS_THRESHOLD; + } else { + return joystick.getRawAxis(axisInt) < -AXIS_THRESHOLD; + } } - + return false; } } diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java b/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java index a4bef2e..a061fc4 100644 --- a/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java +++ b/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java @@ -5,7 +5,7 @@ package org.mayheminc.util; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj2.command.button.*; /** * @author Team1519 @@ -19,16 +19,16 @@ public class JoystickPOVButton extends Button { public static final int SOUTHWEST = 225; public static final int WEST = 270; public static final int NORTHWEST = 315; - + private Joystick joystick; private int desiredPOV; - + public JoystickPOVButton(Joystick stick, int newDesiredPOV) { - joystick = stick; + joystick = stick; desiredPOV = newDesiredPOV; } - + public boolean get() { - return (joystick.getPOV() == desiredPOV); + return (joystick.getPOV() == desiredPOV); } } diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java b/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java index 4b9b719..cb5fc71 100644 --- a/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java +++ b/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java @@ -1,11 +1,10 @@ package org.mayheminc.util; -import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class PidCycle extends InstantCommand -{ - protected void initialize() { -// Robot.pidTuner.cyclePid(); +public class PidCycle extends InstantCommand { + public void initialize() { + // Robot.pidTuner.cyclePid(); } } \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java b/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java index 1748831..bd1975b 100644 --- a/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java +++ b/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java @@ -1,110 +1,105 @@ package org.mayheminc.util; -import edu.wpi.first.wpilibj.buttons.Button; -import edu.wpi.first.wpilibj.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.Button; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** - * This is a class to help tune a PID. - * Another class that extends IPidTunerObject is passed in. - * This interface defines methods that are used to set the P-I-D values. - * This class gets joystick buttons that define the different options. - * - Button 1: cycle through P-I-D values - * - Button 2: increase the value. - * - Button 3: decrease the value. - * - Button 4: cycle through the amount the value changes: 10, 1, .1, .01, .001, .0001 - * This class needs to be called for updateSmartDashboard(). + * This is a class to help tune a PID. Another class that extends + * IPidTunerObject is passed in. This interface defines methods that are used to + * set the P-I-D values. This class gets joystick buttons that define the + * different options. - Button 1: cycle through P-I-D values - Button 2: + * increase the value. - Button 3: decrease the value. - Button 4: cycle through + * the amount the value changes: 10, 1, .1, .01, .001, .0001 This class needs to + * be called for updateSmartDashboard(). + * * @author User * */ -public class PidTuner extends InstantCommand{ +public class PidTuner extends InstantCommand { Button m_PidCycle; Button m_Inc; Button m_Dec; Button m_ValueCycle; PidTunerObject m_pidObj; - - enum PidCycle {P, I, D, F}; + + enum PidCycle { + P, I, D, F + }; + int m_cycle; int m_position; - + // Remember the buttons and call this object when they are pressed. // remember the PID Object so we can get/set the PID values. - public PidTuner(Button b1, Button b2, Button b3, Button b4, PidTunerObject obj) - { - setRunWhenDisabled(true); - + public PidTuner(final Button b1, final Button b2, final Button b3, final Button b4, final PidTunerObject obj) { + b1.whenPressed(this); b2.whenPressed(this); b3.whenPressed(this); b4.whenPressed(this); - + m_PidCycle = b1; m_ValueCycle = b2; m_Inc = b3; m_Dec = b4; - - m_pidObj = obj; + + m_pidObj = obj; + } + + public boolean runsWhenDisabled() { + return true; + } + + // Run the 'instant command'. Determine which command was pressed. + public void initialize() { + if (m_PidCycle.get()) { + RunCycle(); + } + if (m_Inc.get()) { + RunInc(); + } + if (m_Dec.get()) { + RunDec(); + } + if (m_ValueCycle.get()) { + RunValue(); + } } - - // Run the 'instant command'. Determine which command was pressed. - protected void initialize() { - if( m_PidCycle.get()) - { - RunCycle(); - } - if( m_Inc.get()) - { - RunInc(); - } - if( m_Dec.get()) - { - RunDec(); - } - if( m_ValueCycle.get()) - { - RunValue(); - } - } - - // set the P, I, or D that we are changing - public void RunCycle() - { -// System.out.println("PID Tuner: RunCycle"); + + // set the P, I, or D that we are changing + public void RunCycle() { + // System.out.println("PID Tuner: RunCycle"); m_cycle++; m_cycle = m_cycle % 4; } - String getCycleStr() - { - String str[] = {"P", "I", "D", "F"}; - + String getCycleStr() { + final String str[] = { "P", "I", "D", "F" }; + return str[m_cycle]; } - - // calculate the amount to increment, get the value, apply the amount, set the new value - public void RunInc() - { -// System.out.println("PID Tuner: RunInc"); - double amount = calculateAmount(); - + + // calculate the amount to increment, get the value, apply the amount, set the + // new value + public void RunInc() { + // System.out.println("PID Tuner: RunInc"); + final double amount = calculateAmount(); + double value = getValue(); value = value + amount; - setValue(value); + setValue(value); } - + // if m_position is 1, return 10, 0 returns 1, -1 returns 0.1, etc. - double calculateAmount() - { - double retval = Math.pow(10, m_position); + double calculateAmount() { + final double retval = Math.pow(10, m_position); return retval; } - + // based on the cycle, get the P, I, or D. - double getValue() - { - switch(m_cycle) - { + double getValue() { + switch (m_cycle) { case 0: return m_pidObj.getP(); case 1: @@ -116,12 +111,10 @@ public void RunInc() } return 0.0; } - + // based on the cycle, set the P, I, or D. - void setValue(double d) - { - switch(m_cycle) - { + void setValue(final double d) { + switch (m_cycle) { case 0: m_pidObj.setP(d); break; @@ -136,39 +129,36 @@ void setValue(double d) break; } } - - // calculate the amount to decrement, get the value, apply the amount, set the new value - public void RunDec() - { -// System.out.println("PID Tuner: RunDec"); - double amount = calculateAmount(); - + + // calculate the amount to decrement, get the value, apply the amount, set the + // new value + public void RunDec() { + // System.out.println("PID Tuner: RunDec"); + final double amount = calculateAmount(); + double value = getValue(); value = value - amount; - setValue(value); + setValue(value); } - + // Change the amount we are decrementing or incrementing - public void RunValue() - { -// System.out.println("PID Tuner: RunValue"); + public void RunValue() { + // System.out.println("PID Tuner: RunValue"); m_position--; - if( m_position < -4) - { + if (m_position < -4) { m_position = 1; // 10.1234 } } - - public void updateSmartDashboard() - { - SmartDashboard.putNumber("PID Tuner P", m_pidObj.getP()); - SmartDashboard.putNumber("PID Tuner I", m_pidObj.getI()); - SmartDashboard.putNumber("PID Tuner D", m_pidObj.getD()); - SmartDashboard.putNumber("PID Tuner F", m_pidObj.getF()); + + public void updateSmartDashboard() { + SmartDashboard.putNumber("PID Tuner P", m_pidObj.getP()); + SmartDashboard.putNumber("PID Tuner I", m_pidObj.getI()); + SmartDashboard.putNumber("PID Tuner D", m_pidObj.getD()); + SmartDashboard.putNumber("PID Tuner F", m_pidObj.getF()); SmartDashboard.putString("PID Tuner Cycle", getCycleStr()); SmartDashboard.putNumber("PID Tuner Amount", calculateAmount()); } - + } \ No newline at end of file From 74ef3192e2fd9c3cfa5bbe3e2bd57e66b2382585 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 18 Jan 2020 14:54:52 -0500 Subject: [PATCH 009/121] Fixed the build --- 2020-Robot/build/tmp/jar/MANIFEST.MF | 3 - .../org/mayheminc/robot2020/Constants.java | 89 ------ .../mayheminc/robot2020/RobotContainer.java | 207 -------------- .../org/mayheminc/util/AndJoystickButton.java | 23 -- .../util/DisabledOnlyJoystickButton.java | 26 -- .../util/EnabledOnlyJoystickButton.java | 26 -- .../main/java/org/mayheminc/util/History.java | 59 ---- .../mayheminc/util/JoystickAxisButton.java | 91 ------ .../org/mayheminc/util/JoystickPOVButton.java | 34 --- .../org/mayheminc/util/MB1340Ultrasonic.java | 31 --- .../org/mayheminc/util/MayhemTalonSRX.java | 153 ---------- .../org/mayheminc/util/ObjectListener.java | 159 ----------- .../org/mayheminc/util/ObjectLocation.java | 58 ---- .../java/org/mayheminc/util/PidCycle.java | 10 - .../java/org/mayheminc/util/PidTuner.java | 164 ----------- .../org/mayheminc/util/PidTunerObject.java | 13 - .../mayheminc/util/RangeFinder_GP2D120.java | 60 ---- .../org/mayheminc/util/SchedulerManager.java | 107 ------- .../main/java/org/mayheminc/util/Utils.java | 262 ------------------ .../org/mayheminc/robot2020/Constants.java | 33 +++ .../mayheminc/robot2020/RobotContainer.java | 210 ++++++++++++++ .../mayheminc/robot2020/subsystems/Drive.java | 0 .../subsystems/PIDHeadingCorrection.java | 0 .../robot2020/subsystems/PIDHeadingError.java | 0 .../org/mayheminc/util/EventServer/Event.java | 0 .../util/EventServer/EventServer.java | 0 .../util/EventServer/OneTimeEvent.java | 0 .../mayheminc/util/EventServer/TCPServer.java | 0 .../util/EventServer/TMinusEvent.java | 0 .../vendordeps => vendordeps}/navx_frc.json | 0 30 files changed, 243 insertions(+), 1575 deletions(-) delete mode 100644 2020-Robot/build/tmp/jar/MANIFEST.MF delete mode 100644 2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/History.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java delete mode 100644 2020-Robot/src/main/java/org/mayheminc/util/Utils.java create mode 100644 src/main/java/org/mayheminc/robot2020/RobotContainer.java rename {2020-Robot/src => src}/main/java/org/mayheminc/robot2020/subsystems/Drive.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/EventServer/Event.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/EventServer/EventServer.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/EventServer/TCPServer.java (100%) rename {2020-Robot/src => src}/main/java/org/mayheminc/util/EventServer/TMinusEvent.java (100%) rename {2020-Robot/vendordeps => vendordeps}/navx_frc.json (100%) diff --git a/2020-Robot/build/tmp/jar/MANIFEST.MF b/2020-Robot/build/tmp/jar/MANIFEST.MF deleted file mode 100644 index 5c77096..0000000 --- a/2020-Robot/build/tmp/jar/MANIFEST.MF +++ /dev/null @@ -1,3 +0,0 @@ -Manifest-Version: 1.0 -Main-Class: frc.robot.Main - diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java deleted file mode 100644 index 7843674..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/Constants.java +++ /dev/null @@ -1,89 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide - * numerical or boolean constants. This class should not be used for any other - * purpose. All constants should be declared globally (i.e. public static). Do - * not put anything functional in this class. - * - *

- * It is advised to statically import this class (or one of its inner classes) - * wherever the constants are needed, to reduce verbosity. - */ -public final class Constants { - - public final class Talon { - public static final int DRIVE_RIGHT_A = 1; // high current - public static final int DRIVE_RIGHT_B = 2; // high current - public static final int DRIVE_LEFT_A = 3; // high current - public static final int DRIVE_LEFT_B = 4; // high current - - public static final int SHOOTER_WHEEL = 5; // high current - public static final int SHOOTER_FEEDER = 6; - public static final int SHOOTER_TURRET = 7; - public static final int SHOOTER_HOOD = 8; - - public static final int INTAKE_ROLLERS = 9; - public static final int INTAKE_EXTENDER = 10; - - public static final int MAGAZINE_TURNTABLE = 11; - - public static final int CLIMBER_WINCH_LEFT = 12; // high current - public static final int CLIMBER_WINCH_RIGHT = 13; // high current - - public static final int CLIMBER_WALKER_LEFT = 14; - public static final int CLIMBER_WALKER_RIGHT = 15; - - public static final int CONTROL_PANEL_ROLLER = 16; - } - - public final class PDP { - public static final int DRIVE_RIGHT_A = 1; - public static final int DRIVE_RIGHT_B = 2; - public static final int DRIVE_LEFT_A = 3; - public static final int DRIVE_LEFT_B = 4; - } - - public final class GAMEPAD_AXIS { - public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; - public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; - public static final int GAMEPAD_F310_LEFT_TRIGGER = 2; - public static final int GAMEPAD_F310_RIGHT_TRIGGER = 3; - public static final int GAMEPAD_F310_RIGHT_X_AXIS = 4; - public static final int GAMEPAD_F310_RIGHT_Y_AXIS = 5; - } - - public final class GAMEPAD_BUTTION { - public static final int GAMEPAD_F310_A_BUTTON = 1; - public static final int GAMEPAD_F310_B_BUTTON = 2; - public static final int GAMEPAD_F310_X_BUTTON = 3; - public static final int GAMEPAD_F310_Y_BUTTON = 4; - public static final int GAMEPAD_F310_LEFT_BUTTON = 5; - public static final int GAMEPAD_F310_RIGHT_BUTTON = 6; - public static final int GAMEPAD_F310_BACK_BUTTON = 7; - public static final int GAMEPAD_F310_START_BUTTON = 8; - public static final int GAMEPAD_F310_LEFT_STICK_BUTTON = 9; - public static final int GAMEPAD_F310_RIGHT_STICK_BUTTON = 10; - } - - public final class OPERATOR_PAD_AXIS { - public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; - public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; - public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; - public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; - } - - public final class Joysticks { - public static final int DRIVER_GAMEPAD = 0; - public static final int DRIVER_JOYSTICK = 1; - public static final int OPERATOR_GAMEPAD = 2; - public static final int OPERATOR_JOYSTICK = 3; - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java deleted file mode 100644 index 9f6db4a..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ /dev/null @@ -1,207 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020; - -import org.mayheminc.util.*; - -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.XboxController; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.Button; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; - -/** - * This class is where the bulk of the robot should be declared. Since - * Command-based is a "declarative" paradigm, very little robot logic should - * actually be handled in the {@link Robot} periodic methods (other than the - * scheduler calls). Instead, the structure of the robot (including subsystems, - * commands, and button mappings) should be declared here. - */ -public class RobotContainer { - // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); - - private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); - - private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); - - @SuppressWarnings("unused") - private final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger - @SuppressWarnings("unused") - private final Button DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger - @SuppressWarnings("unused") - private final Button DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left Stick Trigger - @SuppressWarnings("unused") - private final Button DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right Stick Trigger - - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); - - private final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); - private final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); - private final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); - private final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); - - private final Button CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, - // plus - // left - // top - // trigger - private final Button CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // A=Green, - // plus - // left - // top - // trigger - - // Driver Control Modes - @SuppressWarnings("unused") - private final Button TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); - @SuppressWarnings("unused") - private final Button TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); - - // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO - // NOT USE HERE!!! - private final Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); - private final JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, - Constants.GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); - private final JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, - Constants.GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); - - @SuppressWarnings("unused") - private final Button DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); - private final Button DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); - private final Button DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1); // Green "A" button - private final Button DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button - private final Button DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button - private final Button DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button - - private final Joystick DRIVER_STICK = new Joystick(Constants.Joysticks.DRIVER_JOYSTICK); - - private final Button DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(DRIVER_STICK, 1); - private final Button DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(DRIVER_STICK, 1); - private final Button DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(DRIVER_STICK, 2); - private final Button DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(DRIVER_STICK, 3); - private final Button DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(DRIVER_STICK, 4); - private final Button DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(DRIVER_STICK, 5); - private final Button DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(DRIVER_STICK, 6); - private final Button DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 7); - private final Button DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(DRIVER_STICK, 8); - private final Button DRIVER_STICK_BUTTON_NINE = new JoystickButton(DRIVER_STICK, 9); - private final Button DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 10); - private final Button DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 11); - public final int DRIVER_STICK_X_AXIS = 0; - public final int DRIVER_STICK_Y_AXIS = 1; - - public static final Joystick OPERATOR_PAD = new Joystick(Constants.Joysticks.OPERATOR_GAMEPAD); - private static final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); - private static final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); - private static final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); - private static final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); - private static final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); - private static final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); - private static final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); - private static final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); - private static final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); - private static final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); - @SuppressWarnings("unused") - private static final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); - private static final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); - @SuppressWarnings("unused") - private static final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); - - // Operator Control Buttons - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - // Configure the button bindings - configureButtonBindings(); - } - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - confiugreDriverStickButtons(); - confiugreDriverPadButtons(); - confiugreOperatorStickButtons(); - confiugreOperatorPadButtons(); - - } - - private void confiugreDriverStickButtons() { - - // DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); - // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); - - // // adjust auto parameters - // DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(1)); - // DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(-1)); - // DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(-1)); - // DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(1)); - - // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner - // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); - - // // zero elements that require zeroing - // DRIVER_STICK_BUTTON_EIGHT.whenPressed(new DriveZeroGyro()); - // DRIVER_STICK_BUTTON_NINE.whenPressed(new Wait(0)); - - } - - private void confiugreDriverPadButtons() { - } - - private void confiugreOperatorStickButtons() { - } - - private void confiugreOperatorPadButtons() { - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous - return m_autoCommand; - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java deleted file mode 100644 index 2c4da30..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/AndJoystickButton.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.mayheminc.util; - -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.button.*; - -public class AndJoystickButton extends Button { - GenericHID joystick1; - GenericHID joystick2; - int buttonNumber1; - int buttonNumber2; - - public AndJoystickButton(GenericHID joystick1, int buttonNumber1, GenericHID joystick2, int buttonNumber2) { - this.joystick1 = joystick1; - this.buttonNumber1 = buttonNumber1; - this.joystick2 = joystick2; - this.buttonNumber2 = buttonNumber2; - } - - public boolean get() { - return joystick1.getRawButton(buttonNumber1) && joystick2.getRawButton(buttonNumber2); - } - -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java deleted file mode 100644 index a76ab1c..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.mayheminc.util; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.button.*; - -/** - * - * @author Team1519 - */ -public class DisabledOnlyJoystickButton extends Button { - - private GenericHID joystick; - private int buttonNumber; - private DriverStation ds; - - public DisabledOnlyJoystickButton(GenericHID joystick, int buttonNumber) { - this.joystick = joystick; - this.buttonNumber = buttonNumber; - ds = DriverStation.getInstance(); - } - - public boolean get() { - return joystick.getRawButton(buttonNumber) && ds.isDisabled(); - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java b/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java deleted file mode 100644 index 9bd9d4a..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.mayheminc.util; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.button.*; - -/** - * - * @author Team1519 - */ -public class EnabledOnlyJoystickButton extends Button { - - private GenericHID joystick; - private int buttonNumber; - private DriverStation ds; - - public EnabledOnlyJoystickButton(GenericHID joystick, int buttonNumber) { - this.joystick = joystick; - this.buttonNumber = buttonNumber; - ds = DriverStation.getInstance(); - } - - public boolean get() { - return joystick.getRawButton(buttonNumber) && ds.isEnabled(); - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/History.java b/2020-Robot/src/main/java/org/mayheminc/util/History.java deleted file mode 100644 index b91707f..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/History.java +++ /dev/null @@ -1,59 +0,0 @@ -package org.mayheminc.util; - -import edu.wpi.first.wpilibj.DriverStation; - -public class History { - private static final int HISTORY_SIZE = 50; - - private double time[] = new double[HISTORY_SIZE]; - private double azimuth[] = new double[HISTORY_SIZE]; - private int index = 0; - - public History() { - // ensure there is at least one element in the history - add(-1.0, 0.0); // make a fictitious element at t=-1 seconds, with heading of 0.0 degrees - } - - public void add(double t, double az) { - - time[index] = t; - azimuth[index] = az; - - index++; - if (index >= HISTORY_SIZE) { - index = 0; - } - } - - public double getAzForTime(double t) { - double az = azimuth[index]; - int i = index - 1; - int count = 0; - - // if (t < 0) { - // DriverStation.reportError("Negative time in history", false); - // return 0.0; // no negative times. - // } - - while (i != index) { - if (i < 0) { - i = HISTORY_SIZE - 1; - } - - if (time[i] <= t) { - az = azimuth[i]; - break; - } - - i--; - count++; - if (count > HISTORY_SIZE) { - DriverStation.reportError("Looking too far back", false); - az = azimuth[index]; - break; - } - } - - return az; - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java b/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java deleted file mode 100644 index 0657aa4..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/JoystickAxisButton.java +++ /dev/null @@ -1,91 +0,0 @@ -/* - * To change this template, choose Tools | Templates - * and open the template in the editor. - */ -package org.mayheminc.util; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.*; - -/** - * - * @author Team1519 - */ -public class JoystickAxisButton extends Button { - public static final int BOTH_WAYS = 1; - public static final int POSITIVE_ONLY = 2; - public static final int NEGATIVE_ONLY = 3; - - private static final double AXIS_THRESHOLD = 0.2; - - private Joystick joystick; - private Joystick.AxisType axis; - private int axisInt; - private int direction; - - private double getAxis(Joystick.AxisType axis) { - switch (axis) { - case kX: - return joystick.getX(); - case kY: - return joystick.getY(); - case kZ: - return joystick.getZ(); - case kThrottle: - return joystick.getThrottle(); - case kTwist: - return joystick.getTwist(); - default: - // Unreachable - return 0.0; - } - } - - public JoystickAxisButton(Joystick stick, Joystick.AxisType axis) { - this(stick, axis, BOTH_WAYS); - } - - public JoystickAxisButton(Joystick stick, Joystick.AxisType axis, int direction) { - joystick = stick; - this.axis = axis; - this.direction = direction; - } - - public JoystickAxisButton(Joystick stick, int axis) { - this(stick, axis, BOTH_WAYS); - } - - public JoystickAxisButton(Joystick stick, int axis, int direction) { - joystick = stick; - this.axis = null; - axisInt = axis; - this.direction = direction; - } - - public boolean get() { - switch (direction) { - case BOTH_WAYS: - if (axis != null) { - return Math.abs(getAxis(axis)) > AXIS_THRESHOLD; - } else { - return Math.abs(joystick.getRawAxis(axisInt)) > AXIS_THRESHOLD; - } - - case POSITIVE_ONLY: - if (axis != null) { - return getAxis(axis) > AXIS_THRESHOLD; - } else { - return joystick.getRawAxis(axisInt) > AXIS_THRESHOLD; - } - - case NEGATIVE_ONLY: - if (axis != null) { - return getAxis(axis) < -AXIS_THRESHOLD; - } else { - return joystick.getRawAxis(axisInt) < -AXIS_THRESHOLD; - } - } - - return false; - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java b/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java deleted file mode 100644 index a061fc4..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/JoystickPOVButton.java +++ /dev/null @@ -1,34 +0,0 @@ -/* - * To change this template, choose Tools | Templates - * and open the template in the editor. - */ -package org.mayheminc.util; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.*; - -/** - * @author Team1519 - */ -public class JoystickPOVButton extends Button { - public static final int NORTH = 0; - public static final int NORTHEAST = 45; - public static final int EAST = 90; - public static final int SOUTHEAST = 135; - public static final int SOUTH = 180; - public static final int SOUTHWEST = 225; - public static final int WEST = 270; - public static final int NORTHWEST = 315; - - private Joystick joystick; - private int desiredPOV; - - public JoystickPOVButton(Joystick stick, int newDesiredPOV) { - joystick = stick; - desiredPOV = newDesiredPOV; - } - - public boolean get() { - return (joystick.getPOV() == desiredPOV); - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java b/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java deleted file mode 100644 index 76442a5..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.mayheminc.util; -import edu.wpi.first.wpilibj.AnalogInput; - -/* - * The Ultrasonic sensor being used is an MB1340. - * The conversion factor being used is (5 volts) / (1024 per cm). - */ - -public class MB1340Ultrasonic { - AnalogInput analogUltrasonic; - final static double voltage = 5.0; - final static double maxADCBins = 1024; - - public MB1340Ultrasonic(int analogPort) { - analogUltrasonic = new AnalogInput(analogPort); - } - - /** - * Get the Distance in cm. - * Full scale is 1024 cm. The minimum is 25cm. - * @return - */ - public double getDistance() { - return (analogUltrasonic.getAverageVoltage() / voltage) * maxADCBins; - } - - public double getDistanceInInches() { - return getDistance() / 2.54; - } - -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java deleted file mode 100644 index d605650..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/MayhemTalonSRX.java +++ /dev/null @@ -1,153 +0,0 @@ -package org.mayheminc.util; - -import com.ctre.phoenix.ErrorCode; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.*; - -public class MayhemTalonSRX extends TalonSRX { - - ControlMode controlMode; - double p; - double i; - double d; - double f; - - public MayhemTalonSRX(int deviceNumber) { - super(deviceNumber); - - this.configNominalOutputForward(0.0, 0); - this.configNominalOutputReverse(0.0, 0); - this.configPeakOutputForward(1.0, 0); - this.configPeakOutputReverse(-1.0, 0); - - this.setNeutralMode(NeutralMode.Coast); - -// this.configContinuousCurrentLimit(0, 0); -// this.configPeakCurrentLimit(0, 0); -// this.configPeakCurrentDuration(0, 0); -// this.configForwardLimitSwitchSource(RemoteLimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled, 0, 0); -// this.configForwardSoftLimitEnable(false, 0); - - // copied from CTRE Example: https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java/Current%20Limit/src/org/usfirst/frc/team217/robot/Robot.java#L37 -// this.configPeakCurrentLimit(80, 10); -// this.configPeakCurrentDuration(60000, 10); /* this is a necessary call to avoid errata. */ -// this.configContinuousCurrentLimit(40, 10); -// this.enableCurrentLimit(true); /* honor initial setting */ - - } - - public ErrorCode config_kP(int slot, double value, int timeout) - { - p = value; - return super.config_kP(slot, value, timeout); - } - - public ErrorCode config_kI(int slot, double value, int timeout) - { - i = value; - return super.config_kI(slot, value, timeout); - } - - public ErrorCode config_kD(int slot, double value, int timeout) - { - d = value; - return super.config_kD(slot, value, timeout); - } - public ErrorCode config_kF(int slot, double value, int timeout) - { - f = value; - return super.config_kF(slot, value, timeout); - } - - public double getP() {return p;} - public double getI() {return i;} - public double getD() {return d;} - public double getF() {return f;} - - public void changeControlMode(ControlMode mode) { - controlMode = mode; - } - - public void set(int deviceID) { - this.set(controlMode, deviceID); - } - - public void setFeedbackDevice(FeedbackDevice feedback) { - this.configSelectedFeedbackSensor(feedback, 0, 1000); - } - - public void reverseSensor(boolean b) { - - } - - public void configNominalOutputVoltage(float f, float g) { - this.configNominalOutputForward(f/12.0, 1000); - this.configNominalOutputReverse(g/12.0, 1000); - } - - public void configPeakOutputVoltage(double d, double e) { - this.configPeakOutputForward(d/12.0, 1000); - this.configPeakOutputReverse(e/12.0, 1000); - - } - - public void setPID(double wheelP, double wheelI, double wheelD, double wheelF, int i, double m_voltageRampRate, - int j) { - this.config_kP(pidProfile, wheelP , 1000); - this.config_kI(pidProfile, wheelI , 1000); - this.config_kD(pidProfile, wheelD , 1000); - this.config_kF(pidProfile, wheelF , 1000); - } - - public double getSetpoint() { - return 0; - } - - public double getError() { - return this.getClosedLoopError(0); - } - - public float getOutputVoltage() { - return (float) this.getMotorOutputVoltage(); - } - - int pidProfile; - public void setProfile(int pidSlot) { - pidProfile = pidSlot; - } - - public void setPID(double pDown, double iDown, double dDown) { - setPID(pDown, iDown, dDown, 0.0, 0, 0.0, 0); - } - - public void setVoltageRampRate(double d) { - // Need to convert volts per second to time - this.configClosedloopRamp(0, 0); - } - - public void enableControl() { - - } - - public void setPosition(int zeroPositionCount) { - this.setSelectedSensorPosition(zeroPositionCount, 0, 1000); - } - - public int getPosition() { - return this.getSelectedSensorPosition(0); - } - - public double getSpeed() { - return this.getSelectedSensorVelocity(0); - } - - public void setEncPosition(int i) { - setPosition(i); - } - - public double get() { - return this.getOutputCurrent(); - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java b/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java deleted file mode 100644 index 5cae99b..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/ObjectListener.java +++ /dev/null @@ -1,159 +0,0 @@ -package org.mayheminc.util; - -import java.util.*; -import java.io.*; -import java.net.*; -import java.nio.*; - -public class ObjectListener extends Thread { - protected static final int MAX_OBJECTS_PER_FRAME = 20; - protected static final int MAYHEM_MAGIC = 0x1519B0B4; - protected static final int MAX_BUFFER = 1500; - protected static final int DEFAULT_PORT = 5810; - - private DatagramSocket socket; - private DatagramPacket packet; - private ByteBuffer buffer; - private int lastFrame = 0; - private ArrayList objList; - private Callback callback = null; - - public interface Callback { - public void objectListenerCallback(int frame, ArrayList objList); - } - - public ObjectListener() throws SocketException { - this(DEFAULT_PORT); - } - - public ObjectListener(int port) throws SocketException { - super("ObjectListener-" + port); - - socket = new DatagramSocket(port); - - byte[] byteBuffer = new byte[MAX_BUFFER]; - packet = new DatagramPacket(byteBuffer, byteBuffer.length); - buffer = ByteBuffer.wrap(byteBuffer); - - objList = new ArrayList(); - } - - public int getLastFrame() { - return lastFrame; - } - - public List getObjectList() { - return objList; - } - - public void setCallback(Callback callback) { - this.callback = callback; - } - - public void run() { - String name = super.getName(); - long lastTimestamp = 0; - - while (true) { - // Receive new datagram - try { - socket.receive(packet); - buffer.rewind(); - } catch (IOException e) { - System.err.println(super.getName() + " encountered an error"); - e.printStackTrace(); - System.err.println(super.getName() + " aborting"); - break; - } - - // Abort if told to do so - if (Thread.interrupted()) - break; - - // Validate packet - int magic = buffer.getInt(); - if (magic != MAYHEM_MAGIC) { - System.err.println(name + ": invalid packet received (magic == 0x" + Integer.toHexString(magic) + ")"); - continue; - } - - // Get information about the update - int frame = buffer.getInt(); - long timestamp = buffer.getLong(); - - // Check for out-of-date data - if (timestamp <= lastTimestamp) { - System.err.println(name + ": timestamp for new frame #" + frame + " (" + timestamp + ") is not newer than that for previous frame #" + lastFrame + " (" + lastTimestamp + "); rejecting out-of-date data"); - lastTimestamp = timestamp; - continue; - } - if (frame <= lastFrame) { - System.err.println(name + ": frame #" + frame + " is earlier than existing frame #" + lastFrame + "; did object detection service restart?"); - } - - // Get list of all objects involved - ArrayList objList = new ArrayList(); - for (int i = 0; i < MAX_OBJECTS_PER_FRAME; i++) { - // Pull the next object from the buffer - ObjectLocation loc = new ObjectLocation(buffer); - - // As soon as one object is none, so are the rest - if (loc.type == ObjectLocation.ObjectTypes.OBJ_NONE) - break; - - // Add object to our list - objList.add(loc); - } - - // Update the list of objects - this.objList = objList; - lastFrame = frame; - lastTimestamp = timestamp; - - // Invoke callback, if applicable - if (callback != null) { - callback.objectListenerCallback(frame, objList); - } - } - - // Clean up - socket.close(); - } - - // Sample implementation for testing and demonstration purposes - public static void main(String[] args) { - ObjectListener listener; - Callback callback = new ObjectListener.Callback() { - public void objectListenerCallback(int frame, ArrayList objList) { - System.out.println("Received notification about objects in frame #" + frame); - for (ObjectLocation loc: objList) { - System.out.println(" " + loc); - } - } - }; - - // Create the listener - try { - listener = new ObjectListener(); - } catch (SocketException e) { - e.printStackTrace(); - return; - } - - // Use the callback implementation - listener.setCallback(callback); - - // Begin listening - System.out.println("Starting object listener...\n"); - listener.start(); - - // Wait forever -- notifications will come from callback - while (true) { - try { - Thread.sleep(600); - } catch (InterruptedException e) { - break; - } - } - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java b/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java deleted file mode 100644 index a903461..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/ObjectLocation.java +++ /dev/null @@ -1,58 +0,0 @@ -package org.mayheminc.util; - -import java.nio.*; -import java.text.DecimalFormat; - -public class ObjectLocation { - public enum ObjectTypes { - OBJ_NONE, - OBJ_CUBE, - OBJ_SCALE_CENTER, - OBJ_SCALE_BLUE, - OBJ_SCALE_RED, - OBJ_SWITCH_RED, - OBJ_SWITCH_BLUE, - OBJ_PORTAL_RED, - OBJ_PORTAL_BLUE, - OBJ_EXCHANGE_RED, - OBJ_EXCHANGE_BLUE, - OBJ_BUMPERS_RED, - OBJ_BUMPERS_BLUE, - OBJ_BUMPERS_CLASS13, - OBJ_BUMPERS_CLASS14, - OBJ_BUMPERS_CLASS15, - OBJ_BUMPERS_CLASS16, - OBJ_BUMPERS_CLASS17, - OBJ_BUMPERS_CLASS18, - OBJ_BUMPERS_CLASS19, - OBJ_BUMPERS_CLASS20, - OBJ_EOL, - }; - - public ObjectTypes type; - public float x; - public float y; - public float width; - public float height; - public float probability; - - public ObjectLocation() { - type = ObjectTypes.OBJ_NONE; - x = y = width = height = probability = 0; - } - - public ObjectLocation(ByteBuffer buffer) { - type = ObjectTypes.values()[buffer.getInt()]; - x = (float)buffer.getInt() / Integer.MAX_VALUE; - y = (float)buffer.getInt() / Integer.MAX_VALUE; - width = (float)buffer.getInt() / Integer.MAX_VALUE; - height = (float)buffer.getInt() / Integer.MAX_VALUE; - probability = (float)buffer.getInt() / Integer.MAX_VALUE; - } - - public String toString() { - DecimalFormat df = new DecimalFormat("0.00"); - - return type.name() + "@" + df.format(x) + "x" + df.format(y) + "+" + df.format(width) + "x" + df.format(height) + "[" + df.format(probability * 100) + "%]"; - } -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java b/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java deleted file mode 100644 index cb5fc71..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/PidCycle.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.mayheminc.util; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class PidCycle extends InstantCommand { - public void initialize() { - // Robot.pidTuner.cyclePid(); - } - -} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java b/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java deleted file mode 100644 index bd1975b..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/PidTuner.java +++ /dev/null @@ -1,164 +0,0 @@ -package org.mayheminc.util; - -import edu.wpi.first.wpilibj2.command.*; -import edu.wpi.first.wpilibj2.command.button.Button; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * This is a class to help tune a PID. Another class that extends - * IPidTunerObject is passed in. This interface defines methods that are used to - * set the P-I-D values. This class gets joystick buttons that define the - * different options. - Button 1: cycle through P-I-D values - Button 2: - * increase the value. - Button 3: decrease the value. - Button 4: cycle through - * the amount the value changes: 10, 1, .1, .01, .001, .0001 This class needs to - * be called for updateSmartDashboard(). - * - * @author User - * - */ -public class PidTuner extends InstantCommand { - Button m_PidCycle; - Button m_Inc; - Button m_Dec; - Button m_ValueCycle; - PidTunerObject m_pidObj; - - enum PidCycle { - P, I, D, F - }; - - int m_cycle; - int m_position; - - // Remember the buttons and call this object when they are pressed. - // remember the PID Object so we can get/set the PID values. - public PidTuner(final Button b1, final Button b2, final Button b3, final Button b4, final PidTunerObject obj) { - - b1.whenPressed(this); - b2.whenPressed(this); - b3.whenPressed(this); - b4.whenPressed(this); - - m_PidCycle = b1; - m_ValueCycle = b2; - m_Inc = b3; - m_Dec = b4; - - m_pidObj = obj; - } - - public boolean runsWhenDisabled() { - return true; - } - - // Run the 'instant command'. Determine which command was pressed. - public void initialize() { - if (m_PidCycle.get()) { - RunCycle(); - } - if (m_Inc.get()) { - RunInc(); - } - if (m_Dec.get()) { - RunDec(); - } - if (m_ValueCycle.get()) { - RunValue(); - } - } - - // set the P, I, or D that we are changing - public void RunCycle() { - // System.out.println("PID Tuner: RunCycle"); - m_cycle++; - m_cycle = m_cycle % 4; - } - - String getCycleStr() { - final String str[] = { "P", "I", "D", "F" }; - - return str[m_cycle]; - } - - // calculate the amount to increment, get the value, apply the amount, set the - // new value - public void RunInc() { - // System.out.println("PID Tuner: RunInc"); - final double amount = calculateAmount(); - - double value = getValue(); - value = value + amount; - setValue(value); - } - - // if m_position is 1, return 10, 0 returns 1, -1 returns 0.1, etc. - double calculateAmount() { - final double retval = Math.pow(10, m_position); - return retval; - } - - // based on the cycle, get the P, I, or D. - double getValue() { - switch (m_cycle) { - case 0: - return m_pidObj.getP(); - case 1: - return m_pidObj.getI(); - case 2: - return m_pidObj.getD(); - case 3: - return m_pidObj.getF(); - } - return 0.0; - } - - // based on the cycle, set the P, I, or D. - void setValue(final double d) { - switch (m_cycle) { - case 0: - m_pidObj.setP(d); - break; - case 1: - m_pidObj.setI(d); - break; - case 2: - m_pidObj.setD(d); - break; - case 3: - m_pidObj.setF(d); - break; - } - } - - // calculate the amount to decrement, get the value, apply the amount, set the - // new value - public void RunDec() { - // System.out.println("PID Tuner: RunDec"); - final double amount = calculateAmount(); - - double value = getValue(); - value = value - amount; - setValue(value); - } - - // Change the amount we are decrementing or incrementing - public void RunValue() { - // System.out.println("PID Tuner: RunValue"); - - m_position--; - if (m_position < -4) { - m_position = 1; // 10.1234 - } - } - - public void updateSmartDashboard() { - SmartDashboard.putNumber("PID Tuner P", m_pidObj.getP()); - SmartDashboard.putNumber("PID Tuner I", m_pidObj.getI()); - SmartDashboard.putNumber("PID Tuner D", m_pidObj.getD()); - SmartDashboard.putNumber("PID Tuner F", m_pidObj.getF()); - - SmartDashboard.putString("PID Tuner Cycle", getCycleStr()); - SmartDashboard.putNumber("PID Tuner Amount", calculateAmount()); - } - -} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java b/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java deleted file mode 100644 index d9a59d0..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/PidTunerObject.java +++ /dev/null @@ -1,13 +0,0 @@ -package org.mayheminc.util; - -public interface PidTunerObject { - - double getP(); - double getI(); - double getD(); - double getF(); - void setP(double d); - void setI(double d); - void setD(double d); - void setF(double d); -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java b/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java deleted file mode 100644 index 061fcbc..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java +++ /dev/null @@ -1,60 +0,0 @@ -package org.mayheminc.util; - -import edu.wpi.first.wpilibj.AnalogInput; - -// http://media.digikey.com/pdf/Data%20Sheets/Sharp%20PDFs/GP2D120.pdf - -public class RangeFinder_GP2D120 { - - private AnalogInput m_channel; - private static final int HISTORY_BUFFER_SIZE = 5; - private double m_historyBuffer[]=new double[HISTORY_BUFFER_SIZE]; - private int m_historyBufferIndex; - private double m_filteredVoltage; - - public RangeFinder_GP2D120(int channel, int index) { - m_channel = new AnalogInput(channel); - m_filteredVoltage = 0.0; - } - - public void periodic() { - double min = m_historyBuffer[0]; - m_historyBuffer[m_historyBufferIndex]= m_channel.getVoltage(); //return channel.getVoltage(); - m_historyBufferIndex++; - - if (m_historyBufferIndex>=HISTORY_BUFFER_SIZE) { - m_historyBufferIndex=0; - } - - for (int i=1; i= OBJECT_IS_SEEN_VOLTAGE; - } - - // had been 0.80 prior to 31 Jan - private static final double OBJECT_IS_CLOSE_VOLTAGE = 0.80; - public boolean isObjectClose() { - return getFilteredVoltage() >= OBJECT_IS_CLOSE_VOLTAGE; - } - - // added on 11 Feb 2015 to allow "floating off tote" - private static final double OBJECT_IS_VERY_CLOSE_VOLTAGE = 1.60; - public boolean isObjectVeryClose() { - return getFilteredVoltage() >= OBJECT_IS_VERY_CLOSE_VOLTAGE; - } - -} \ No newline at end of file diff --git a/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java b/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java deleted file mode 100644 index 26300ab..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/SchedulerManager.java +++ /dev/null @@ -1,107 +0,0 @@ -/** - * Copyright (c) 2015-2019 Team 501 - The PowerKnights. All Rights Reserved. - * Open Source Software - May be modified and shared by FRC teams. The code must - * be accompanied by the Team 501 BSD license file in the root directory of the - * project. You may also obtain a copy of it from the following: - * http://www.opensource.org/licenses/bsd-license.php. - * - * See (Git) repository metadata for author and revision history for this file. - **/ - -package org.mayheminc.util; - -// import org.slf4j.Logger; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.NetworkTableType; -import edu.wpi.first.networktables.NetworkTableValue; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -// import riolog.RioLogger; - -/** - * - */ -public class SchedulerManager // implements ITelemetryProvider -{ - - /* Our classes logger */ - // @SuppressWarnings("unused") - // private static final Logger logger = - // RioLogger.getLogger(SchedulerManager.class.getName()); - - /** Singleton instance of class for all to use **/ - private static SchedulerManager ourInstance; - /** Name of our subsystem **/ - private final static String myName = "Running Commands";// TelemetryNames.Scheduler.name; - - /** - * Constructs instance of the subsystem. Assumed to be called before any usage - * of the subsystem; and verifies only called once. Allows controlled startup - * sequencing of the robot and all it's subsystems. - **/ - public static synchronized void constructInstance() { - // SmartDashboard.putBoolean(TelemetryNames.Scheduler.status, false); - - if (ourInstance != null) { - throw new IllegalStateException(myName + " Already Constructed"); - } - ourInstance = new SchedulerManager(); - - // SmartDashboard.putBoolean(TelemetryNames.Scheduler.status, true); - } - - /** - * Returns the singleton instance of the subsystem in the form of the - * Interface that is defined for it. If it hasn't been constructed yet, - * throws an IllegalStateException. - * - * @return singleton instance of subsystem - **/ - public static SchedulerManager getInstance() { - if (ourInstance == null) { - throw new IllegalStateException(myName + " Not Constructed Yet"); - } - return ourInstance; - } - - private final NetworkTable liveWindow; - private final NetworkTable ungrouped; - private final NetworkTable scheduler; - private final StringBuilder names; - - public SchedulerManager() { - NetworkTableInstance inst = NetworkTableInstance.getDefault(); - liveWindow = inst.getTable("LiveWindow"); - ungrouped = liveWindow.getSubTable("Ungrouped"); - scheduler = ungrouped.getSubTable("Scheduler"); - - names = new StringBuilder(); - } - - // @Override - public void updateTelemetry() { - NetworkTableEntry namesEntry = scheduler.getEntry("Names"); - NetworkTableValue namesValue = namesEntry.getValue(); - if ((namesValue == null)) { - SmartDashboard.putString("Current Commands", "namesValue is null"); - return; - } - - if ((namesValue.getType() != NetworkTableType.kStringArray)) { - - SmartDashboard.putString("Current Commands", "namesValue is " + namesValue.getType()); - return; - } - - names.setLength(0); - for (String name : namesValue.getStringArray()) { - names.append(name).append(": "); - } - - SmartDashboard.putString("Current Commands", names.toString()); - } - -} diff --git a/2020-Robot/src/main/java/org/mayheminc/util/Utils.java b/2020-Robot/src/main/java/org/mayheminc/util/Utils.java deleted file mode 100644 index edcabda..0000000 --- a/2020-Robot/src/main/java/org/mayheminc/util/Utils.java +++ /dev/null @@ -1,262 +0,0 @@ -/* - * Utility class with useful methods in it - */ -package org.mayheminc.util; - -/** - * - * @author Team1519 - */ -public class Utils { - - public static double oneDecimalPlace(double d) { - return ((double) ((int) (d * 10))) / 10; - } - - public static double twoDecimalPlaces(double d) { - return ((double) ((int) (d * 100))) / 100; - } - - public static double threeDecimalPlaces(double d) { - return ((double) ((int) (d * 1000))) / 1000; - } - - public static double fourDecimalPlaces(double d) { - return ((double) ((int) (d * 10000))) / 10000; - } - - /** - * Returns the arc tangent of an angle, in the range of - * -Math.PI/2 through - * Math.PI/2. Special cases:

  • If the argument is - * NaN, then the result is - * NaN.
  • If the argument is zero, then the result is a zero - * with the same sign as the argument.

A result must be within 1 - * ulp of the correctly rounded result. Results must be semi-monotonic. - * - * @param a - the value whose arc tangent is to be returned. - * @return the arc tangent of the argument. - */ - public static double atan(double a) { - // Special cases. - if (Double.isNaN(a)) { - return Double.NaN; - } - - if (a == 0.0) { - return a; - } - - // Compute the arc tangent. - boolean negative = false; - boolean greaterThanOne = false; - int i = 0; - - if (a < 0.0) { - a = -a; - negative = true; - } - - if (a > 1.0) { - a = 1.0 / a; - greaterThanOne = true; - } - - double t; - - for (; a > PIover12; a *= t) { - i++; - t = a + ATAN_CONSTANT; - t = 1.0 / t; - a *= ATAN_CONSTANT; - a--; - } - - double aSquared = a * a; - - double arcTangent = aSquared + 1.4087812; - arcTangent = 0.55913709 / arcTangent; - arcTangent += 0.60310578999999997; - arcTangent -= 0.051604539999999997 * aSquared; - arcTangent *= a; - - for (; i > 0; i--) { - arcTangent += PIover6; - } - - if (greaterThanOne) { - arcTangent = PIover2 - arcTangent; - } - - if (negative) { - arcTangent = -arcTangent; - } - - return arcTangent; - } - /** - * Constant for PI divided by 2. - */ - public static final double PIover2 = Math.PI / 2; - /** - * Constant for PI divided by 4. - */ - public static final double PIover4 = Math.PI / 4; - /** - * Constant for PI divided by 6. - */ - public static final double PIover6 = Math.PI / 6; - /** - * Constant for PI divided by 12. - */ - public static final double PIover12 = Math.PI / 12; - /** - * Constant used in the - * atan calculation. - */ - private static final double ATAN_CONSTANT = 1.732050807569; - - /** - * Converts rectangular coordinates (x, y) to polar (r, theta). This - * method computes the phase theta by computing an arc tangent of y/x - * in the range of -pi to pi. Special cases:

  • If - * either argument is - * NaN, then the result is - * NaN.
  • If the first argument is positive zero and the - * second argument is positive, or the first argument is positive and finite - * and the second argument is positive infinity, then the result is positive - * zero.
  • If the first argument is negative zero and the second argument - * is positive, or the first argument is negative and finite and the second - * argument is positive infinity, then the result is negative zero.
  • If - * the first argument is positive zero and the second argument is negative, - * or the first argument is positive and finite and the second argument is - * negative infinity, then the result is the - * double value closest to pi.
  • If the first argument - * is negative zero and the second argument is negative, or the first - * argument is negative and finite and the second argument is negative - * infinity, then the result is the - * double value closest to -pi.
  • If the first - * argument is positive and the second argument is positive zero or negative - * zero, or the first argument is positive infinity and the second argument - * is finite, then the result is the - * double value closest to pi/2.
  • If the first - * argument is negative and the second argument is positive zero or negative - * zero, or the first argument is negative infinity and the second argument - * is finite, then the result is the - * double value closest to -pi/2.
  • If both arguments - * are positive infinity, then the result is the double value closest to - * pi/4.
  • If the first argument is positive infinity and the - * second argument is negative infinity, then the result is the double value - * closest to 3*pi/4.
  • If the first argument is negative infinity - * and the second argument is positive infinity, then the result is the - * double value closest to -pi/4.
  • If both arguments are negative - * infinity, then the result is the double value closest to -3*pi/4. - *

A result must be within 2 ulps of the correctly rounded result. - * Results must be semi-monotonic. - * - * @param y - the ordinate coordinate - * @param x - the abscissa coordinate - * @return the theta component of the point (r, theta) in - * polar coordinates that corresponds to the point (x, y) in Cartesian - * coordinates. - */ - public static double atan2(double y, double x) { - // Special cases. - if (Double.isNaN(y) || Double.isNaN(x)) { - return Double.NaN; - } else if (Double.isInfinite(y)) { - if (y > 0.0) // Positive infinity - { - if (Double.isInfinite(x)) { - if (x > 0.0) { - return PIover4; - } else { - return 3.0 * PIover4; - } - } else if (x != 0.0) { - return PIover2; - } - } else // Negative infinity - { - if (Double.isInfinite(x)) { - if (x > 0.0) { - return -PIover4; - } else { - return -3.0 * PIover4; - } - } else if (x != 0.0) { - return -PIover2; - } - } - } else if (y == 0.0) { - if (x > 0.0) { - return y; - } else if (x < 0.0) { - return Math.PI; - } - else { - return 0; - } - } else if (Double.isInfinite(x)) { - if (x > 0.0) // Positive infinity - { - if (y > 0.0) { - return 0.0; - } else if (y < 0.0) { - return -0.0; - } - } else // Negative infinity - { - if (y > 0.0) { - return Math.PI; - } else if (y < 0.0) { - return -Math.PI; - } - } - } else if (x == 0.0) { - if (y > 0.0) { - return PIover2; - } else if (y < 0.0) { - return -PIover2; - } - else { - // Should not get her with y test above - return 0; - } - } - - - // Implementation a simple version ported from a PASCAL implementation: - // http://everything2.com/index.pl?node_id=1008481 - - double arcTangent; - - // Use arctan() avoiding division by zero. - if (Math.abs(x) > Math.abs(y)) { - arcTangent = atan(y / x); - } else { - arcTangent = atan(x / y); // -PI/4 <= a <= PI/4 - - if (arcTangent < 0) { - arcTangent = -PIover2 - arcTangent; // a is negative, so we're adding - } else { - arcTangent = PIover2 - arcTangent; - } - } - - // Adjust result to be from [-PI, PI] - if (x < 0) { - if (y < 0) { - arcTangent = arcTangent - Math.PI; - } else { - arcTangent = arcTangent + Math.PI; - } - } - - return arcTangent; - } - - public static boolean isBetween(double value, double upper, double lower){ - return value < upper && value > lower; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index ebba44a..7843674 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -18,6 +18,39 @@ * wherever the constants are needed, to reduce verbosity. */ public final class Constants { + + public final class Talon { + public static final int DRIVE_RIGHT_A = 1; // high current + public static final int DRIVE_RIGHT_B = 2; // high current + public static final int DRIVE_LEFT_A = 3; // high current + public static final int DRIVE_LEFT_B = 4; // high current + + public static final int SHOOTER_WHEEL = 5; // high current + public static final int SHOOTER_FEEDER = 6; + public static final int SHOOTER_TURRET = 7; + public static final int SHOOTER_HOOD = 8; + + public static final int INTAKE_ROLLERS = 9; + public static final int INTAKE_EXTENDER = 10; + + public static final int MAGAZINE_TURNTABLE = 11; + + public static final int CLIMBER_WINCH_LEFT = 12; // high current + public static final int CLIMBER_WINCH_RIGHT = 13; // high current + + public static final int CLIMBER_WALKER_LEFT = 14; + public static final int CLIMBER_WALKER_RIGHT = 15; + + public static final int CONTROL_PANEL_ROLLER = 16; + } + + public final class PDP { + public static final int DRIVE_RIGHT_A = 1; + public static final int DRIVE_RIGHT_B = 2; + public static final int DRIVE_LEFT_A = 3; + public static final int DRIVE_LEFT_B = 4; + } + public final class GAMEPAD_AXIS { public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java new file mode 100644 index 0000000..b36004b --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -0,0 +1,210 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020; + +import org.mayheminc.util.*; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; +// import frc.robot.commands.ExampleCommand; +// import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems and commands are defined here... + // private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + + // private final ExampleCommand m_autoCommand = new + // ExampleCommand(m_exampleSubsystem); + + private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); + + @SuppressWarnings("unused") + private final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger + @SuppressWarnings("unused") + private final Button DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger + @SuppressWarnings("unused") + private final Button DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left Stick Trigger + @SuppressWarnings("unused") + private final Button DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right Stick Trigger + + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); + + private final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); + private final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); + private final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); + private final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); + + private final Button CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, + // plus + // left + // top + // trigger + private final Button CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // A=Green, + // plus + // left + // top + // trigger + + // Driver Control Modes + @SuppressWarnings("unused") + private final Button TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); + @SuppressWarnings("unused") + private final Button TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); + + // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO + // NOT USE HERE!!! + private final Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); + private final JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + Constants.GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + private final JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + Constants.GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + + @SuppressWarnings("unused") + private final Button DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); + private final Button DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); + private final Button DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1); // Green "A" button + private final Button DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button + private final Button DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button + private final Button DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button + + private final Joystick DRIVER_STICK = new Joystick(Constants.Joysticks.DRIVER_JOYSTICK); + + private final Button DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(DRIVER_STICK, 1); + private final Button DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(DRIVER_STICK, 1); + private final Button DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(DRIVER_STICK, 2); + private final Button DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(DRIVER_STICK, 3); + private final Button DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(DRIVER_STICK, 4); + private final Button DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(DRIVER_STICK, 5); + private final Button DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(DRIVER_STICK, 6); + private final Button DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 7); + private final Button DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(DRIVER_STICK, 8); + private final Button DRIVER_STICK_BUTTON_NINE = new JoystickButton(DRIVER_STICK, 9); + private final Button DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 10); + private final Button DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 11); + public final int DRIVER_STICK_X_AXIS = 0; + public final int DRIVER_STICK_Y_AXIS = 1; + + public static final Joystick OPERATOR_PAD = new Joystick(Constants.Joysticks.OPERATOR_GAMEPAD); + private static final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); + private static final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); + private static final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); + private static final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); + private static final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); + private static final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); + private static final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); + private static final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); + private static final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); + private static final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); + @SuppressWarnings("unused") + private static final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); + private static final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); + @SuppressWarnings("unused") + private static final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); + + // Operator Control Buttons + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + confiugreDriverStickButtons(); + confiugreDriverPadButtons(); + confiugreOperatorStickButtons(); + confiugreOperatorPadButtons(); + + } + + private void confiugreDriverStickButtons() { + + // DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); + // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); + + // // adjust auto parameters + // DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(1)); + // DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(-1)); + // DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(-1)); + // DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(1)); + + // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner + // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); + + // // zero elements that require zeroing + // DRIVER_STICK_BUTTON_EIGHT.whenPressed(new DriveZeroGyro()); + // DRIVER_STICK_BUTTON_NINE.whenPressed(new Wait(0)); + + } + + private void confiugreDriverPadButtons() { + } + + private void confiugreOperatorStickButtons() { + } + + private void confiugreOperatorPadButtons() { + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // An ExampleCommand will run in autonomous + return null; + } +} diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java rename to src/main/java/org/mayheminc/robot2020/subsystems/Drive.java diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java rename to src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java diff --git a/2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java rename to src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/Event.java b/src/main/java/org/mayheminc/util/EventServer/Event.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/EventServer/Event.java rename to src/main/java/org/mayheminc/util/EventServer/Event.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/EventServer.java b/src/main/java/org/mayheminc/util/EventServer/EventServer.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/EventServer/EventServer.java rename to src/main/java/org/mayheminc/util/EventServer/EventServer.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java b/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java rename to src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/TCPServer.java b/src/main/java/org/mayheminc/util/EventServer/TCPServer.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/EventServer/TCPServer.java rename to src/main/java/org/mayheminc/util/EventServer/TCPServer.java diff --git a/2020-Robot/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java b/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java similarity index 100% rename from 2020-Robot/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java rename to src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java diff --git a/2020-Robot/vendordeps/navx_frc.json b/vendordeps/navx_frc.json similarity index 100% rename from 2020-Robot/vendordeps/navx_frc.json rename to vendordeps/navx_frc.json From 7b85802141b4f9f9bda3ed26573b4b562707f4f4 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 18 Jan 2020 14:59:47 -0500 Subject: [PATCH 010/121] Create Intake.java --- .../robot2020/subsystems/Intake.java | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Intake.java diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java new file mode 100644 index 0000000..dd4a4ef --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + /** + * Creates a new Intake. + */ + public Intake() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From dcb82ee15f8178ad240b0ac67a8b025521d2dbc6 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Sat, 18 Jan 2020 15:05:23 -0500 Subject: [PATCH 011/121] added shooter subsystem --- .../mayheminc/robot2020/RobotContainer.java | 364 +++++++++--------- .../robot2020/subsystems/Shooter.java | 17 + 2 files changed, 200 insertions(+), 181 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index b36004b..c34b0e8 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.Button; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import org.mayheminc.robot2020.subsystems.*; /** * This class is where the bulk of the robot should be declared. Since @@ -26,185 +27,186 @@ * commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - // private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); - - // private final ExampleCommand m_autoCommand = new - // ExampleCommand(m_exampleSubsystem); - - private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); - - @SuppressWarnings("unused") - private final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger - @SuppressWarnings("unused") - private final Button DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger - @SuppressWarnings("unused") - private final Button DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left Stick Trigger - @SuppressWarnings("unused") - private final Button DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right Stick Trigger - - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); - - private final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); - private final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); - private final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); - private final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); - - private final Button CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, - // plus - // left - // top - // trigger - private final Button CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // A=Green, - // plus - // left - // top - // trigger - - // Driver Control Modes - @SuppressWarnings("unused") - private final Button TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); - @SuppressWarnings("unused") - private final Button TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); - - // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO - // NOT USE HERE!!! - private final Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); - private final JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, - Constants.GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); - private final JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, - Constants.GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); - - @SuppressWarnings("unused") - private final Button DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); - private final Button DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); - private final Button DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1); // Green "A" button - private final Button DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button - private final Button DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button - private final Button DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button - - private final Joystick DRIVER_STICK = new Joystick(Constants.Joysticks.DRIVER_JOYSTICK); - - private final Button DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(DRIVER_STICK, 1); - private final Button DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(DRIVER_STICK, 1); - private final Button DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(DRIVER_STICK, 2); - private final Button DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(DRIVER_STICK, 3); - private final Button DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(DRIVER_STICK, 4); - private final Button DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(DRIVER_STICK, 5); - private final Button DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(DRIVER_STICK, 6); - private final Button DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 7); - private final Button DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(DRIVER_STICK, 8); - private final Button DRIVER_STICK_BUTTON_NINE = new JoystickButton(DRIVER_STICK, 9); - private final Button DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 10); - private final Button DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 11); - public final int DRIVER_STICK_X_AXIS = 0; - public final int DRIVER_STICK_Y_AXIS = 1; - - public static final Joystick OPERATOR_PAD = new Joystick(Constants.Joysticks.OPERATOR_GAMEPAD); - private static final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); - private static final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); - private static final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); - private static final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); - private static final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); - private static final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); - private static final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); - private static final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); - private static final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); - private static final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); - @SuppressWarnings("unused") - private static final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); - private static final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); - @SuppressWarnings("unused") - private static final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); - - // Operator Control Buttons - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - // Configure the button bindings - configureButtonBindings(); - } - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - confiugreDriverStickButtons(); - confiugreDriverPadButtons(); - confiugreOperatorStickButtons(); - confiugreOperatorPadButtons(); - - } - - private void confiugreDriverStickButtons() { - - // DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); - // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); - - // // adjust auto parameters - // DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(1)); - // DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(-1)); - // DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(-1)); - // DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(1)); - - // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner - // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); - - // // zero elements that require zeroing - // DRIVER_STICK_BUTTON_EIGHT.whenPressed(new DriveZeroGyro()); - // DRIVER_STICK_BUTTON_NINE.whenPressed(new Wait(0)); - - } - - private void confiugreDriverPadButtons() { - } - - private void confiugreOperatorStickButtons() { - } - - private void confiugreOperatorPadButtons() { - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous - return null; - } + // The robot's subsystems and commands are defined here... + // private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + + // private final ExampleCommand m_autoCommand = new + // ExampleCommand(m_exampleSubsystem); + + private final Shooter m_shooter = new Shooter(); + private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); + + @SuppressWarnings("unused") + private final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger + @SuppressWarnings("unused") + private final Button DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger + @SuppressWarnings("unused") + private final Button DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left Stick Trigger + @SuppressWarnings("unused") + private final Button DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right Stick Trigger + + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); + @SuppressWarnings("unused") + private final JoystickPOVButton DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); + + private final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); + private final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); + private final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); + private final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); + + private final Button CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, + // plus + // left + // top + // trigger + private final Button CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // A=Green, + // plus + // left + // top + // trigger + + // Driver Control Modes + @SuppressWarnings("unused") + private final Button TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); + @SuppressWarnings("unused") + private final Button TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); + + // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO + // NOT USE HERE!!! + private final Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); + private final JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + Constants.GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + private final JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + Constants.GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + + @SuppressWarnings("unused") + private final Button DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); + private final Button DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, + Constants.GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); + private final Button DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1); // Green "A" button + private final Button DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button + private final Button DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button + private final Button DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button + + private final Joystick DRIVER_STICK = new Joystick(Constants.Joysticks.DRIVER_JOYSTICK); + + private final Button DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(DRIVER_STICK, 1); + private final Button DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(DRIVER_STICK, 1); + private final Button DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(DRIVER_STICK, 2); + private final Button DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(DRIVER_STICK, 3); + private final Button DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(DRIVER_STICK, 4); + private final Button DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(DRIVER_STICK, 5); + private final Button DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(DRIVER_STICK, 6); + private final Button DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 7); + private final Button DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(DRIVER_STICK, 8); + private final Button DRIVER_STICK_BUTTON_NINE = new JoystickButton(DRIVER_STICK, 9); + private final Button DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 10); + private final Button DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 11); + public final int DRIVER_STICK_X_AXIS = 0; + public final int DRIVER_STICK_Y_AXIS = 1; + + public static final Joystick OPERATOR_PAD = new Joystick(Constants.Joysticks.OPERATOR_GAMEPAD); + private static final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); + private static final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); + private static final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); + private static final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); + private static final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); + private static final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); + private static final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); + private static final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); + private static final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); + private static final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); + @SuppressWarnings("unused") + private static final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); + private static final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); + @SuppressWarnings("unused") + private static final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); + + // Operator Control Buttons + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + @SuppressWarnings("unused") + private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + confiugreDriverStickButtons(); + confiugreDriverPadButtons(); + confiugreOperatorStickButtons(); + confiugreOperatorPadButtons(); + + } + + private void confiugreDriverStickButtons() { + + // DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); + // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); + + // // adjust auto parameters + // DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(1)); + // DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(-1)); + // DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(-1)); + // DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(1)); + + // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner + // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); + + // // zero elements that require zeroing + // DRIVER_STICK_BUTTON_EIGHT.whenPressed(new DriveZeroGyro()); + // DRIVER_STICK_BUTTON_NINE.whenPressed(new Wait(0)); + + } + + private void confiugreDriverPadButtons() { + } + + private void confiugreOperatorStickButtons() { + } + + private void confiugreOperatorPadButtons() { + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // An ExampleCommand will run in autonomous + return null; + } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java new file mode 100644 index 0000000..61c589e --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -0,0 +1,17 @@ +package org.mayheminc.robot2020.subsystems; + +import com.kauailabs.navx.frc.*; +import org.mayheminc.util.History; + +import edu.wpi.first.wpilibj.*; +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.Utils; + +public class Shooter extends SubsystemBase { +} \ No newline at end of file From 67eee4a3fc76f6fec95f1b10ab02f606da5f5eef Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 18 Jan 2020 15:09:02 -0500 Subject: [PATCH 012/121] Update Shooter.java --- .../org/mayheminc/robot2020/subsystems/Shooter.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 61c589e..d004818 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -14,4 +14,16 @@ import org.mayheminc.util.Utils; public class Shooter extends SubsystemBase { + + /** + * Creates a new Shooter. + */ + public Shooter() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } } \ No newline at end of file From 359a2c1c7e2cec9d135bb41fb36e6d296c813394 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Sat, 18 Jan 2020 15:15:00 -0500 Subject: [PATCH 013/121] added Climber and Magazine --- .../robot2020/subsystems/Climber.java | 24 +++++++++++++++++++ .../robot2020/subsystems/Magazine.java | 24 +++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Climber.java create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java new file mode 100644 index 0000000..659e48a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climber extends SubsystemBase { + /** + * Creates a new Climber. + */ + public Climber() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java new file mode 100644 index 0000000..c2be8b3 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Magazine extends SubsystemBase { + /** + * Creates a new Magazine. + */ + public Magazine() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From 08842a6435d1e65f7f58080991cc7cfbb72c2b0d Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Sat, 18 Jan 2020 15:45:05 -0500 Subject: [PATCH 014/121] added Talons to all subsystems --- .../java/org/mayheminc/robot2020/RobotContainer.java | 2 ++ .../java/org/mayheminc/robot2020/subsystems/Climber.java | 9 +++++++++ .../java/org/mayheminc/robot2020/subsystems/Intake.java | 8 ++++++++ .../org/mayheminc/robot2020/subsystems/Magazine.java | 6 +++++- .../java/org/mayheminc/robot2020/subsystems/Shooter.java | 6 ++++++ 5 files changed, 30 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index c34b0e8..16f06cb 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -33,6 +33,8 @@ public class RobotContainer { // private final ExampleCommand m_autoCommand = new // ExampleCommand(m_exampleSubsystem); + private final Climber m_Climber = new Climber(); + private final Magazine m_Magazine = new Magazine(); private final Shooter m_shooter = new Shooter(); private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 659e48a..1d741cb 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -7,9 +7,18 @@ package org.mayheminc.robot2020.subsystems; +import org.mayheminc.robot2020.Constants; + +import org.mayheminc.util.MayhemTalonSRX; + import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { + private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT); + private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT); + private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); + private final MayhemTalonSRX walkerRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); + /** * Creates a new Climber. */ diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index dd4a4ef..38ae27c 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -7,9 +7,17 @@ package org.mayheminc.robot2020.subsystems; +import org.mayheminc.robot2020.Constants; + +import org.mayheminc.util.MayhemTalonSRX; + import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase { + + private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_ROLLERS); + private final MayhemTalonSRX extenderTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_EXTENDER); + /** * Creates a new Intake. */ diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java index c2be8b3..bf2a5dd 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java @@ -9,12 +9,16 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.mayheminc.robot2020.Constants; +import org.mayheminc.util.MayhemTalonSRX; + public class Magazine extends SubsystemBase { + private final MayhemTalonSRX turntableTalon = new MayhemTalonSRX(Constants.Talon.MAGAZINE_TURNTABLE); + /** * Creates a new Magazine. */ public Magazine() { - } @Override diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index d004818..d827798 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -1,6 +1,8 @@ package org.mayheminc.robot2020.subsystems; import com.kauailabs.navx.frc.*; + +import org.mayheminc.robot2020.Constants; import org.mayheminc.util.History; import edu.wpi.first.wpilibj.*; @@ -14,6 +16,10 @@ import org.mayheminc.util.Utils; public class Shooter extends SubsystemBase { + private final MayhemTalonSRX shooterWheelTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL); + private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); + private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); + private final MayhemTalonSRX feederTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_FEEDER); /** * Creates a new Shooter. From a6fa7e9b38e4cd0aa4cbba1ae60fb95cb2e3ed2b Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Sat, 18 Jan 2020 16:40:03 -0500 Subject: [PATCH 015/121] first take on tallon setting --- .../robot2020/subsystems/Intake.java | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 38ae27c..8a3bac8 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -7,6 +7,9 @@ package org.mayheminc.robot2020.subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; + import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; @@ -22,7 +25,24 @@ public class Intake extends SubsystemBase { * Creates a new Intake. */ public Intake() { + rollerTalon.setNeutralMode(NeutralMode.Coast); + rollerTalon.configNominalOutputVoltage(+0.0f, -0.0f); + rollerTalon.configPeakOutputVoltage(+12.0, -12.0); + + } + + public void setRollers(double power) { + rollerTalon.set(ControlMode.PercentOutput, power); + + } + + public void setExtender(boolean b) { + if (b) { + extenderTalon.set(ControlMode.Position, 90); + } else { + extenderTalon.set(ControlMode.Position, 10); + } } @Override From 4d621f21601dd583578ab7a3764025662169106f Mon Sep 17 00:00:00 2001 From: robertdeml Date: Mon, 20 Jan 2020 19:13:57 -0500 Subject: [PATCH 016/121] added a diabled command. added shooter pid skeleton. --- .../commands/RobotDisabledCommand.java | 23 +++++++++++++++++++ .../robot2020/subsystems/Shooter.java | 14 +++++++++++ 2 files changed, 37 insertions(+) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/RobotDisabledCommand.java diff --git a/src/main/java/org/mayheminc/robot2020/commands/RobotDisabledCommand.java b/src/main/java/org/mayheminc/robot2020/commands/RobotDisabledCommand.java new file mode 100644 index 0000000..57885d0 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/RobotDisabledCommand.java @@ -0,0 +1,23 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class RobotDisabledCommand extends CommandBase { + /** + * Creates a new RobotDisabledCommand. + */ + public RobotDisabledCommand() { + // Use addRequirements() here to declare subsystem dependencies. + } + + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index d827798..2d26929 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -25,11 +25,25 @@ public class Shooter extends SubsystemBase { * Creates a new Shooter. */ public Shooter() { + configureTurretTalon(); + } + void configureTurretTalon() { + turretTalon.config_kP(0, 1.0, 0); + turretTalon.config_kI(0, 1.0, 0); + turretTalon.config_kD(0, 1.0, 0); + turretTalon.config_kF(0, 1.0, 0); + turretTalon.changeControlMode(ControlMode.Position); + turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); } @Override public void periodic() { // This method will be called once per scheduler run } + + public void setTurretPosition(double pos) { + turretTalon.set(ControlMode.Position, pos); + } + } \ No newline at end of file From 4b2ab272a9ad17c04c365d0cca60031a3dcc0ef4 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Mon, 20 Jan 2020 19:23:13 -0500 Subject: [PATCH 017/121] added drive to robot container --- src/main/java/org/mayheminc/robot2020/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 16f06cb..e98e49b 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -37,7 +37,7 @@ public class RobotContainer { private final Magazine m_Magazine = new Magazine(); private final Shooter m_shooter = new Shooter(); private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); - + private final Drive m_drive = new Drive(); @SuppressWarnings("unused") private final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger @SuppressWarnings("unused") From fddec81a68b18fd52f389ba5f59eab2337cfcdf2 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Mon, 20 Jan 2020 19:24:12 -0500 Subject: [PATCH 018/121] Added Drive Zero Gyro. Also updated the pid values to a safe value. --- .../robot2020/commands/DriveZeroGyro.java | 44 +++++++++++++++++++ .../robot2020/subsystems/Shooter.java | 6 +-- 2 files changed, 47 insertions(+), 3 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java new file mode 100644 index 0000000..f0dd43d --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java @@ -0,0 +1,44 @@ +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Drive; + +// import org.mayheminc.robot2019.Robot; + +// import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class DriveZeroGyro extends RobotDisabledCommand { + Drive m_drive; + + public DriveZeroGyro(Drive drive) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + addRequirements(drive); + m_drive = drive; + } + + // Called just before this Command runs the first time + public void initialize() { + m_drive.zeroHeadingGyro(0.0); + } + + // Called repeatedly when this Command is scheduled to run + public void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + public 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/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 2d26929..72bea31 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -30,9 +30,9 @@ public Shooter() { void configureTurretTalon() { turretTalon.config_kP(0, 1.0, 0); - turretTalon.config_kI(0, 1.0, 0); - turretTalon.config_kD(0, 1.0, 0); - turretTalon.config_kF(0, 1.0, 0); + turretTalon.config_kI(0, 0.0, 0); + turretTalon.config_kD(0, 0.0, 0); + turretTalon.config_kF(0, 0.0, 0); turretTalon.changeControlMode(ControlMode.Position); turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); } From e2991d42009b1a10c4bef98494859d32e675c6b8 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Mon, 20 Jan 2020 20:52:59 -0500 Subject: [PATCH 019/121] filing out the magazine and shooter --- .../mayheminc/robot2020/subsystems/Magazine.java | 11 +++++++++++ .../mayheminc/robot2020/subsystems/Shooter.java | 14 ++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java index bf2a5dd..1f6421b 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java @@ -25,4 +25,15 @@ public Magazine() { public void periodic() { // This method will be called once per scheduler run } + + public void setTurntableSpeed(boolean b) { + + if (b) { + turntableTalon.set(1); + } else { + turntableTalon.set(0); + } + + }; + } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 2d26929..cc2319c 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -44,6 +44,20 @@ public void periodic() { public void setTurretPosition(double pos) { turretTalon.set(ControlMode.Position, pos); + + } + + public void setHoodPosition(double pos) { + hoodTalon.set(ControlMode.Position, pos); + } + public void setFeederPosition(double pos) { + feederTalon.set(ControlMode.Position, pos); + + } + + public void setShooterWheelPosition(double pos) { + shooterWheelTalon.set(ControlMode.Position, pos); + } } \ No newline at end of file From 2ea98aa777bedd7122b0ec681a9a9d9c411b447c Mon Sep 17 00:00:00 2001 From: robertdeml Date: Mon, 20 Jan 2020 20:54:17 -0500 Subject: [PATCH 020/121] Filling in the autonomous stuff Added a couple autonomous routines. Added the autonomous subsystem. --- .../mayheminc/robot2020/RobotContainer.java | 19 ++++- .../autonomousroutines/DriveStraight.java | 26 +++++++ .../autonomousroutines/StayStill.java | 25 ++++++ .../commands/DriveStraightOnHeading.java | 78 +++++++++++++++++++ .../robot2020/subsystems/Autonomous.java | 76 ++++++++++++++++++ 5 files changed, 221 insertions(+), 3 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index e98e49b..96c68f1 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -12,11 +12,15 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; -// import frc.robot.commands.ExampleCommand; -// import frc.robot.subsystems.ExampleSubsystem; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.button.Button; import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import java.util.LinkedList; + +import org.mayheminc.robot2020.autonomousroutines.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; import org.mayheminc.robot2020.subsystems.*; /** @@ -36,8 +40,12 @@ public class RobotContainer { private final Climber m_Climber = new Climber(); private final Magazine m_Magazine = new Magazine(); private final Shooter m_shooter = new Shooter(); - private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); private final Drive m_drive = new Drive(); + LinkedList m_autonomousPrograms = new LinkedList(); + private final Autonomous m_autonomous = new Autonomous(); + + private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); + @SuppressWarnings("unused") private final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger @SuppressWarnings("unused") @@ -157,6 +165,11 @@ public class RobotContainer { public RobotContainer() { // Configure the button bindings configureButtonBindings(); + + m_autonomousPrograms.push(new StayStill(this.m_drive)); + m_autonomousPrograms.push(new DriveStraight(this.m_drive)); + + m_autonomous.setAutonomousPrograms(this.m_autonomousPrograms); } /** diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java new file mode 100644 index 0000000..9fac532 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Drive; + +import edu.wpi.first.wpilibj2.command.*; + +public class DriveStraight extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public DriveStraight(Drive drive) { + + addCommands(new DriveZeroGyro(drive)); + addCommands(new DriveStraightOnHeading(drive, 0.5, DistanceUnits.ENCODER_TICKS, 1000, 0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java new file mode 100644 index 0000000..22663de --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java @@ -0,0 +1,25 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.robot2020.autonomousroutines; + +import edu.wpi.first.wpilibj2.command.*; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.subsystems.Drive; + +/** + * + * @author Team1519 + */ +public class StayStill extends SequentialCommandGroup { + + public StayStill(Drive drive) { + + // Perform needed initialization + addCommands(new DriveZeroGyro(drive)); + + // ALL DONE! + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java new file mode 100644 index 0000000..71216ba --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java @@ -0,0 +1,78 @@ +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Drive; + +// import org.mayheminc.robot2019.Robot; +// import org.mayheminc.robot2019.subsystems.Drive; + +// import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class DriveStraightOnHeading extends CommandBase { + + double m_targetPower; + double m_desiredDisplacement; + double m_desiredHeading; + Drive m_drive; + + public enum DistanceUnits { + ENCODER_TICKS, INCHES + }; + + public DriveStraightOnHeading(Drive drive, double arg_targetSpeed, double arg_distance, double heading) { + this(drive, arg_targetSpeed, DistanceUnits.INCHES, arg_distance, heading); + } + + /** + * + * @param arg_targetPower +/- motor power [-1.0, +1.0] + * @param arg_distance Distance in encoder counts + */ + public DriveStraightOnHeading(Drive drive, double arg_targetSpeed, DistanceUnits units, double arg_distance, + double heading) { + addRequirements(drive); + + if (units == DistanceUnits.INCHES) { + arg_distance = arg_distance / Drive.DISTANCE_PER_PULSE; + } + m_drive = drive; + m_targetPower = arg_targetSpeed; + m_desiredDisplacement = Math.abs(arg_distance); + m_desiredHeading = heading; + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + m_drive.saveInitialWheelDistance(); + m_drive.setDesiredHeading(m_desiredHeading); + // System.out.println("Starting Routine: Drive Straight On Heading"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + public void execute() { + m_drive.speedRacerDrive(m_targetPower, 0, false); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + int displacement = (int) m_drive.getWheelDistance(); + + displacement = Math.abs(displacement); + // System.out.println("displacement" + displacement); + + return (displacement >= m_desiredDisplacement); + } + + // Called once after isFinished returns true + @Override + public void end(boolean interrupted) { + m_drive.stop(); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java new file mode 100644 index 0000000..5d0c099 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java @@ -0,0 +1,76 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.robot2020.subsystems; + +import java.util.ArrayList; +import java.util.LinkedList; + +import org.mayheminc.robot2020.autonomousroutines.*; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * + * @author Team1519 + */ +public class Autonomous extends SubsystemBase { + + private LinkedList autonomousPrograms = new LinkedList(); + + private int programNumber = 0; // 0 = Do nothing + private int delay = 0; + + public Autonomous() { + } + + public void setAutonomousPrograms(LinkedList programs) { + autonomousPrograms = programs; + } + + public CommandBase getSelectedProgram() { + return autonomousPrograms.get(programNumber); + } + + public int getDelay() { + return delay; + } + + public void adjustProgramNumber(final int delta) { + programNumber += delta; + if (programNumber < 0) { + programNumber = autonomousPrograms.size() - 1; + } else if (programNumber >= autonomousPrograms.size()) { + programNumber = 0; + } + updateSmartDashboard(); + } + + private final int MAX_DELAY = 9; + + public void adjustDelay(final int delta) { + delay += delta; + if (delay < 0) { + delay = 0; + } else if (delay > MAX_DELAY) { + delay = MAX_DELAY; + } + updateSmartDashboard(); + } + + private StringBuffer sb = new StringBuffer(); + + public void updateSmartDashboard() { + sb.setLength(0); + sb.append(programNumber + " " + autonomousPrograms.get(programNumber).getName()); + sb.append(" "); + // SmartDashboard.putString("Auto Prog", sb.toString()); + // SmartDashboard.putNumber("Auto Delay", delay); + } + + public String toString() { + return autonomousPrograms.get(programNumber).getName(); + } +} From 502f751f593515966e359ecf160896572b68411c Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Mon, 20 Jan 2020 21:53:21 -0500 Subject: [PATCH 021/121] configuring the talons in the climber and the magazine --- .../robot2020/subsystems/Climber.java | 37 ++++++++++++++++++- .../robot2020/subsystems/Magazine.java | 5 +++ 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 1d741cb..9bd651c 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -7,6 +7,9 @@ package org.mayheminc.robot2020.subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; + import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; @@ -23,6 +26,21 @@ public class Climber extends SubsystemBase { * Creates a new Climber. */ public Climber() { + winchLeft.setNeutralMode(NeutralMode.Brake); + winchLeft.configNominalOutputVoltage(+0.0f, -0.0f); + winchLeft.configPeakOutputVoltage(+12.0, -12.0); + + winchRight.setNeutralMode(NeutralMode.Brake); + winchRight.configNominalOutputVoltage(+0.0f, -0.0f); + winchRight.configPeakOutputVoltage(+12.0, -12.0); + + walkerRight.setNeutralMode(NeutralMode.Brake); + walkerRight.configNominalOutputVoltage(+0.0f, -0.0f); + walkerRight.configPeakOutputVoltage(+12.0, -12.0); + + walkerLeft.setNeutralMode(NeutralMode.Brake); + walkerLeft.configNominalOutputVoltage(+0.0f, -0.0f); + walkerLeft.configPeakOutputVoltage(+12.0, -12.0); } @@ -30,4 +48,21 @@ public Climber() { public void periodic() { // This method will be called once per scheduler run } -} + + public void setWinchLeftSpeed(double power) { + winchLeft.set(ControlMode.Velocity, power); + } + + public void setWinchRightSpeed(double power) { + winchRight.set(ControlMode.Velocity, power); + } + + public void setWalkerLeftSpeed(double power) { + walkerLeft.set(ControlMode.Velocity, power); + } + + public void setWalkerRightSpeed(double power) { + walkerRight.set(ControlMode.Velocity, power); + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java index 1f6421b..9bf717d 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java @@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix.motorcontrol.NeutralMode; + import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; @@ -19,6 +21,9 @@ public class Magazine extends SubsystemBase { * Creates a new Magazine. */ public Magazine() { + turntableTalon.setNeutralMode(NeutralMode.Coast); + turntableTalon.configNominalOutputVoltage(+0.0f, -0.0f); + turntableTalon.configPeakOutputVoltage(+12.0, -12.0); } @Override From 320a650feac09031e822fec431f973d3cb17875c Mon Sep 17 00:00:00 2001 From: robertdeml Date: Mon, 20 Jan 2020 21:54:41 -0500 Subject: [PATCH 022/121] Refactored robot container Separated the joystick buttons to separate classes. Added Auto Adjust commands. --- .../org/mayheminc/robot2020/Constants.java | 29 ---- .../mayheminc/robot2020/RobotContainer.java | 142 +++--------------- .../commands/SelectAutonomousDelay.java | 39 +++++ .../commands/SelectAutonomousProgram.java | 30 ++++ .../org/mayheminc/util/MayhemDriverPad.java | 105 +++++++++++++ .../org/mayheminc/util/MayhemDriverStick.java | 44 ++++++ .../org/mayheminc/util/MayhemOperatorPad.java | 75 +++++++++ 7 files changed, 311 insertions(+), 153 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousDelay.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousProgram.java create mode 100644 src/main/java/org/mayheminc/util/MayhemDriverPad.java create mode 100644 src/main/java/org/mayheminc/util/MayhemDriverStick.java create mode 100644 src/main/java/org/mayheminc/util/MayhemOperatorPad.java diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 7843674..741a039 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -51,35 +51,6 @@ public final class PDP { public static final int DRIVE_LEFT_B = 4; } - public final class GAMEPAD_AXIS { - public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; - public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; - public static final int GAMEPAD_F310_LEFT_TRIGGER = 2; - public static final int GAMEPAD_F310_RIGHT_TRIGGER = 3; - public static final int GAMEPAD_F310_RIGHT_X_AXIS = 4; - public static final int GAMEPAD_F310_RIGHT_Y_AXIS = 5; - } - - public final class GAMEPAD_BUTTION { - public static final int GAMEPAD_F310_A_BUTTON = 1; - public static final int GAMEPAD_F310_B_BUTTON = 2; - public static final int GAMEPAD_F310_X_BUTTON = 3; - public static final int GAMEPAD_F310_Y_BUTTON = 4; - public static final int GAMEPAD_F310_LEFT_BUTTON = 5; - public static final int GAMEPAD_F310_RIGHT_BUTTON = 6; - public static final int GAMEPAD_F310_BACK_BUTTON = 7; - public static final int GAMEPAD_F310_START_BUTTON = 8; - public static final int GAMEPAD_F310_LEFT_STICK_BUTTON = 9; - public static final int GAMEPAD_F310_RIGHT_STICK_BUTTON = 10; - } - - public final class OPERATOR_PAD_AXIS { - public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; - public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; - public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; - public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; - } - public final class Joysticks { public static final int DRIVER_GAMEPAD = 0; public static final int DRIVER_JOYSTICK = 1; diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 96c68f1..1e1906b 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -20,7 +20,7 @@ import java.util.LinkedList; import org.mayheminc.robot2020.autonomousroutines.*; -import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.subsystems.*; /** @@ -41,123 +41,11 @@ public class RobotContainer { private final Magazine m_Magazine = new Magazine(); private final Shooter m_shooter = new Shooter(); private final Drive m_drive = new Drive(); - LinkedList m_autonomousPrograms = new LinkedList(); private final Autonomous m_autonomous = new Autonomous(); - private final Joystick DRIVER_PAD = new Joystick(Constants.Joysticks.DRIVER_GAMEPAD); - - @SuppressWarnings("unused") - private final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger - @SuppressWarnings("unused") - private final Button DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger - @SuppressWarnings("unused") - private final Button DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left Stick Trigger - @SuppressWarnings("unused") - private final Button DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right Stick Trigger - - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); - @SuppressWarnings("unused") - private final JoystickPOVButton DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); - - private final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); - private final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); - private final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); - private final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); - - private final Button CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, - // plus - // left - // top - // trigger - private final Button CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // A=Green, - // plus - // left - // top - // trigger - - // Driver Control Modes - @SuppressWarnings("unused") - private final Button TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); - @SuppressWarnings("unused") - private final Button TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); - - // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO - // NOT USE HERE!!! - private final Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); - private final JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, - Constants.GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); - private final JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, - Constants.GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); - - @SuppressWarnings("unused") - private final Button DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); - private final Button DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, - Constants.GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); - private final Button DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1); // Green "A" button - private final Button DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button - private final Button DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button - private final Button DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button - - private final Joystick DRIVER_STICK = new Joystick(Constants.Joysticks.DRIVER_JOYSTICK); - - private final Button DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(DRIVER_STICK, 1); - private final Button DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(DRIVER_STICK, 1); - private final Button DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(DRIVER_STICK, 2); - private final Button DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(DRIVER_STICK, 3); - private final Button DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(DRIVER_STICK, 4); - private final Button DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(DRIVER_STICK, 5); - private final Button DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(DRIVER_STICK, 6); - private final Button DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 7); - private final Button DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(DRIVER_STICK, 8); - private final Button DRIVER_STICK_BUTTON_NINE = new JoystickButton(DRIVER_STICK, 9); - private final Button DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 10); - private final Button DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(DRIVER_STICK, 11); - public final int DRIVER_STICK_X_AXIS = 0; - public final int DRIVER_STICK_Y_AXIS = 1; - - public static final Joystick OPERATOR_PAD = new Joystick(Constants.Joysticks.OPERATOR_GAMEPAD); - private static final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); - private static final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); - private static final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); - private static final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); - private static final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); - private static final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); - private static final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); - private static final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); - private static final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); - private static final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); - @SuppressWarnings("unused") - private static final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); - private static final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); - @SuppressWarnings("unused") - private static final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); - - // Operator Control Buttons - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - @SuppressWarnings("unused") - private static final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - Constants.OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + private final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(Constants.Joysticks.DRIVER_JOYSTICK); + private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(Constants.Joysticks.OPERATOR_GAMEPAD); + private final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(Constants.Joysticks.DRIVER_GAMEPAD); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -165,11 +53,17 @@ public class RobotContainer { public RobotContainer() { // Configure the button bindings configureButtonBindings(); + configureAutonomousPrograms(); + } + + private void configureAutonomousPrograms() { + LinkedList autonomousPrograms = new LinkedList(); + + autonomousPrograms.push(new StayStill(this.m_drive)); + autonomousPrograms.push(new DriveStraight(this.m_drive)); - m_autonomousPrograms.push(new StayStill(this.m_drive)); - m_autonomousPrograms.push(new DriveStraight(this.m_drive)); + m_autonomous.setAutonomousPrograms(autonomousPrograms); - m_autonomous.setAutonomousPrograms(this.m_autonomousPrograms); } /** @@ -192,10 +86,10 @@ private void confiugreDriverStickButtons() { // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); // // adjust auto parameters - // DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(1)); - // DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(-1)); - // DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(-1)); - // DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(m_autonomous, 1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(m_autonomous, -1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(m_autonomous, -1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(m_autonomous, 1)); // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); @@ -222,6 +116,6 @@ private void confiugreOperatorPadButtons() { */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - return null; + return m_autonomous.getCurrentCommand(); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousDelay.java b/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousDelay.java new file mode 100644 index 0000000..db6127f --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousDelay.java @@ -0,0 +1,39 @@ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Autonomous; + +/** + * + * @author Team1519 + */ +public class SelectAutonomousDelay extends RobotDisabledCommand { + + private int delta; + private Autonomous auto; + + public SelectAutonomousDelay(Autonomous auto, int delta) { + addRequirements(auto); + this.auto = auto; + this.delta = delta; + } + + // Called just before this Command runs the first time + public void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + public void execute() { + auto.adjustDelay(delta); + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + public void end(boolean interrupted) { + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousProgram.java b/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousProgram.java new file mode 100644 index 0000000..f7d4201 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousProgram.java @@ -0,0 +1,30 @@ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Autonomous; + +/** + * + * @author Team1519 + */ +public class SelectAutonomousProgram extends RobotDisabledCommand { + + private int m_delta; + private Autonomous auto; + + public SelectAutonomousProgram(Autonomous auto, int delta) { + addRequirements(auto); + this.auto = auto; + m_delta = delta; + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + public void end(boolean interrupted) { + this.auto.adjustProgramNumber(m_delta); + } +} diff --git a/src/main/java/org/mayheminc/util/MayhemDriverPad.java b/src/main/java/org/mayheminc/util/MayhemDriverPad.java new file mode 100644 index 0000000..5cfd763 --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemDriverPad.java @@ -0,0 +1,105 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +public class MayhemDriverPad { + + public final class GAMEPAD_BUTTION { + public static final int GAMEPAD_F310_A_BUTTON = 1; + public static final int GAMEPAD_F310_B_BUTTON = 2; + public static final int GAMEPAD_F310_X_BUTTON = 3; + public static final int GAMEPAD_F310_Y_BUTTON = 4; + public static final int GAMEPAD_F310_LEFT_BUTTON = 5; + public static final int GAMEPAD_F310_RIGHT_BUTTON = 6; + public static final int GAMEPAD_F310_BACK_BUTTON = 7; + public static final int GAMEPAD_F310_START_BUTTON = 8; + public static final int GAMEPAD_F310_LEFT_STICK_BUTTON = 9; + public static final int GAMEPAD_F310_RIGHT_STICK_BUTTON = 10; + } + + public final class GAMEPAD_AXIS { + public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; + public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; + public static final int GAMEPAD_F310_LEFT_TRIGGER = 2; + public static final int GAMEPAD_F310_RIGHT_TRIGGER = 3; + public static final int GAMEPAD_F310_RIGHT_X_AXIS = 4; + public static final int GAMEPAD_F310_RIGHT_Y_AXIS = 5; + } + + public Joystick DRIVER_PAD; + + public Button DRIVER_PAD_BUTTON_FIVE; + public Button DRIVER_PAD_BUTTON_SIX; + public Button DRIVER_PAD_LEFT_STICK_BUTTON; + public Button DRIVER_PAD_RIGHT_STICK_BUTTON; + + public JoystickPOVButton DRIVER_PAD_D_PAD_UP; + public JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT; + public JoystickPOVButton DRIVER_PAD_D_PAD_DOWN; + public JoystickPOVButton DRIVER_PAD_D_PAD_LEFT; + public Button CLIMB_L3_BUTTON_PAIR; + public Button CLIMB_L2_BUTTON_PAIR; + + // Driver Control Modes + public Button TOGGLE_CLOSED_LOOP_MODE_BUTTON; + public Button TOGGLE_FOD_BUTTON; + + // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO + // NOT USE HERE!!! + public Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON; + public JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON; + public JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON; + + public Button DRIVER_PAD_BACK_BUTTON; + public Button DRIVER_PAD_START_BUTTON; + public Button DRIVER_PAD_GREEN_BUTTON; + public Button DRIVER_PAD_RED_BUTTON; + public Button DRIVER_PAD_BLUE_BUTTON; + public Button DRIVER_PAD_YELLOW_BUTTON; + + public MayhemDriverPad(int port) { + + DRIVER_PAD = new Joystick(port); + + DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger + DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger + DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left + // Stick + // Trigger + DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right + // Stick + // Trigger + + DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); + DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); + DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); + DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); + CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, plus left top trigger + CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON);// A=Green, plus left top trigger + + // Driver Control Modes + TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); + TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); + + // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO + // NOT USE HERE!!! + DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); + DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + + DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); + DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); + DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1); // Green "A" button + DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button + DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button + DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button + + } +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/MayhemDriverStick.java b/src/main/java/org/mayheminc/util/MayhemDriverStick.java new file mode 100644 index 0000000..fd45990 --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemDriverStick.java @@ -0,0 +1,44 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; + +public class MayhemDriverStick { + + public Joystick Joystick; + + public Button DRIVER_STICK_BUTTON_ONE_DISABLED; + public Button DRIVER_STICK_BUTTON_ONE_ENABLED; + public Button DRIVER_STICK_BUTTON_TWO; + public Button DRIVER_STICK_BUTTON_THREE; + public Button DRIVER_STICK_BUTTON_FOUR; + public Button DRIVER_STICK_BUTTON_FIVE; + public Button DRIVER_STICK_BUTTON_SIX; + public Button DRIVER_STICK_BUTTON_SEVEN; + public Button DRIVER_STICK_BUTTON_EIGHT; + public Button DRIVER_STICK_BUTTON_NINE; + public Button DRIVER_STICK_BUTTON_TEN; + public Button DRIVER_STICK_BUTTON_ELEVEN; + + public int DRIVER_STICK_X_AXIS = 0; + public int DRIVER_STICK_Y_AXIS = 1; + + public MayhemDriverStick(int port) { + this.Joystick = new Joystick(port); + + DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(Joystick, 1); + DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(Joystick, 1); + DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(Joystick, 2); + DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(Joystick, 3); + DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(Joystick, 4); + DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(Joystick, 5); + DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(Joystick, 6); + DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(Joystick, 7); + DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(Joystick, 8); + DRIVER_STICK_BUTTON_NINE = new JoystickButton(Joystick, 9); + DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(Joystick, 10); + DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(Joystick, 11); + } +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java new file mode 100644 index 0000000..5a06bee --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java @@ -0,0 +1,75 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +public class MayhemOperatorPad { + + public Joystick OPERATOR_PAD; + public Button OPERATOR_PAD_BUTTON_ONE; + public Button OPERATOR_PAD_BUTTON_TWO; + public Button OPERATOR_PAD_BUTTON_THREE; + public Button OPERATOR_PAD_BUTTON_FOUR; + public Button OPERATOR_PAD_BUTTON_FIVE; + public Button OPERATOR_PAD_BUTTON_SIX; + public Button OPERATOR_PAD_BUTTON_SEVEN; + public Button OPERATOR_PAD_BUTTON_EIGHT; + public Button OPERATOR_PAD_BUTTON_NINE; + public Button OPERATOR_PAD_BUTTON_TEN; + public Button OPERATOR_PAD_BUTTON_ELEVEN; + public Button OPERATOR_PAD_BUTTON_TWELVE; + public Button FORCE_FIRE_BUTTON; + + public JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT; + public JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT; + public JoystickPOVButton OPERATOR_PAD_D_PAD_UP; + public JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN; + + // Operator Control Buttons + public JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP; + public JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN; + public JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP; + public JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN; + + public final class OPERATOR_PAD_AXIS { + public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; + public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; + public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; + public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; + } + + public MayhemOperatorPad(int port) { + + OPERATOR_PAD = new Joystick(port); + OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); + OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); + OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); + OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); + OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); + OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); + OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); + OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); + OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); + OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); + OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); + OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); + FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); + + OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); + OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); + OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); + OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); + + // Operator Control Buttons + OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, + JoystickAxisButton.NEGATIVE_ONLY); + OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, + JoystickAxisButton.POSITIVE_ONLY); + OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, + JoystickAxisButton.NEGATIVE_ONLY); + OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + + } +} From d332ce6ffd7ea86b189fa6a89f5f918978adbfc6 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Mon, 20 Jan 2020 22:42:22 -0500 Subject: [PATCH 023/121] speed commands added shooter speed commands. added operator stick --- .../mayheminc/robot2020/RobotContainer.java | 15 +++--- .../commands/ShooterAdjustWheel.java | 50 +++++++++++++++++++ .../robot2020/commands/ShooterSetWheel.java | 50 +++++++++++++++++++ .../robot2020/subsystems/Autonomous.java | 8 +-- .../robot2020/subsystems/Shooter.java | 30 +++++++++-- .../mayheminc/util/MayhemOperatorStick.java | 12 +++++ 6 files changed, 150 insertions(+), 15 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java create mode 100644 src/main/java/org/mayheminc/util/MayhemOperatorStick.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 1e1906b..d01cd99 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -10,12 +10,9 @@ import org.mayheminc.util.*; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.button.Button; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; import java.util.LinkedList; @@ -44,9 +41,12 @@ public class RobotContainer { private final Autonomous m_autonomous = new Autonomous(); private final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(Constants.Joysticks.DRIVER_JOYSTICK); - private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(Constants.Joysticks.OPERATOR_GAMEPAD); private final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(Constants.Joysticks.DRIVER_GAMEPAD); + private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(Constants.Joysticks.OPERATOR_GAMEPAD); + private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick( + Constants.Joysticks.OPERATOR_JOYSTICK); + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -57,7 +57,7 @@ public RobotContainer() { } private void configureAutonomousPrograms() { - LinkedList autonomousPrograms = new LinkedList(); + LinkedList autonomousPrograms = new LinkedList(); autonomousPrograms.push(new StayStill(this.m_drive)); autonomousPrograms.push(new DriveStraight(this.m_drive)); @@ -107,6 +107,9 @@ private void confiugreOperatorStickButtons() { } private void confiugreOperatorPadButtons() { + OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new ShooterAdjustWheel(m_shooter, 100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new ShooterAdjustWheel(m_shooter, -100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterSetWheel(m_shooter, 1000)); } /** diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java new file mode 100644 index 0000000..efa0be1 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterAdjustWheel extends CommandBase { + Shooter m_shooter; + double m_adjust; + + /** + * Creates a new ShooterAdjustWheel. + */ + public ShooterAdjustWheel(Shooter shooter, double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(shooter); + + m_shooter = shooter; + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_shooter.setShooterWheelSpeed(m_shooter.getShooterWheelSpeed() + m_adjust); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java new file mode 100644 index 0000000..f30e714 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Shooter; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterSetWheel extends CommandBase { + Shooter m_shooter; + double m_adjust; + + /** + * Creates a new ShooterSetWheel. + */ + public ShooterSetWheel(Shooter shooter, double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(shooter); + + m_shooter = shooter; + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_shooter.setShooterWheelSpeed(m_adjust); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java index 5d0c099..3a7d67f 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java @@ -9,7 +9,7 @@ import org.mayheminc.robot2020.autonomousroutines.*; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** @@ -18,7 +18,7 @@ */ public class Autonomous extends SubsystemBase { - private LinkedList autonomousPrograms = new LinkedList(); + private LinkedList autonomousPrograms = new LinkedList(); private int programNumber = 0; // 0 = Do nothing private int delay = 0; @@ -26,11 +26,11 @@ public class Autonomous extends SubsystemBase { public Autonomous() { } - public void setAutonomousPrograms(LinkedList programs) { + public void setAutonomousPrograms(LinkedList programs) { autonomousPrograms = programs; } - public CommandBase getSelectedProgram() { + public Command getSelectedProgram() { return autonomousPrograms.get(programNumber); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 185b500..846e4df 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -26,6 +26,16 @@ public class Shooter extends SubsystemBase { */ public Shooter() { configureTurretTalon(); + configureWheelTalon(); + } + + private void configureWheelTalon() { + shooterWheelTalon.config_kP(0, 1.0, 0); + shooterWheelTalon.config_kI(0, 0.0, 0); + shooterWheelTalon.config_kD(0, 0.0, 0); + shooterWheelTalon.config_kF(0, 0.0, 0); + shooterWheelTalon.changeControlMode(ControlMode.Velocity); + shooterWheelTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); } void configureTurretTalon() { @@ -40,24 +50,34 @@ void configureTurretTalon() { @Override public void periodic() { // This method will be called once per scheduler run + UpdateDashboard(); + } + + private void UpdateDashboard() { + SmartDashboard.putNumber("shooter speed", shooterWheelTalon.get()); } public void setTurretPosition(double pos) { turretTalon.set(ControlMode.Position, pos); + } + public double getTurretPosition() { + return turretTalon.get(); } public void setHoodPosition(double pos) { hoodTalon.set(ControlMode.Position, pos); - } - public void setFeederPosition(double pos) { - feederTalon.set(ControlMode.Position, pos); + public void setFeederSpeed(double pos) { + feederTalon.set(ControlMode.PercentOutput, pos); + } + public void setShooterWheelSpeed(double pos) { + shooterWheelTalon.set(ControlMode.Velocity, pos); } - public void setShooterWheelPosition(double pos) { - shooterWheelTalon.set(ControlMode.Position, pos); + public double getShooterWheelSpeed() { + return shooterWheelTalon.get(); } } \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/MayhemOperatorStick.java b/src/main/java/org/mayheminc/util/MayhemOperatorStick.java new file mode 100644 index 0000000..fa1a842 --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemOperatorStick.java @@ -0,0 +1,12 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; + +public class MayhemOperatorStick { + + public Joystick OPERATOR_STICK; + + public MayhemOperatorStick(int port) { + OPERATOR_STICK = new Joystick(port); + } +} \ No newline at end of file From 79ba1fc80e9598f39b857a45d80ba0afb963eb08 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Wed, 22 Jan 2020 01:45:35 -0500 Subject: [PATCH 024/121] Fixed spelling. Added hood talon config. --- .../mayheminc/robot2020/RobotContainer.java | 30 ++++++++----------- .../robot2020/subsystems/Shooter.java | 17 +++++++++++ 2 files changed, 30 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index d01cd99..11e151d 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -29,16 +29,12 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... - // private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); - // private final ExampleCommand m_autoCommand = new - // ExampleCommand(m_exampleSubsystem); - - private final Climber m_Climber = new Climber(); - private final Magazine m_Magazine = new Magazine(); - private final Shooter m_shooter = new Shooter(); - private final Drive m_drive = new Drive(); - private final Autonomous m_autonomous = new Autonomous(); + public final Climber m_climber = new Climber(); + public final Magazine m_magazine = new Magazine(); + public final Shooter m_shooter = new Shooter(); + public final Drive m_drive = new Drive(); + public final Autonomous m_autonomous = new Autonomous(); private final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(Constants.Joysticks.DRIVER_JOYSTICK); private final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(Constants.Joysticks.DRIVER_GAMEPAD); @@ -73,14 +69,14 @@ private void configureAutonomousPrograms() { * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - confiugreDriverStickButtons(); - confiugreDriverPadButtons(); - confiugreOperatorStickButtons(); - confiugreOperatorPadButtons(); + configureDriverStickButtons(); + configureDriverPadButtons(); + configureOperatorStickButtons(); + configureOperatorPadButtons(); } - private void confiugreDriverStickButtons() { + private void configureDriverStickButtons() { // DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); @@ -100,13 +96,13 @@ private void confiugreDriverStickButtons() { } - private void confiugreDriverPadButtons() { + private void configureDriverPadButtons() { } - private void confiugreOperatorStickButtons() { + private void configureOperatorStickButtons() { } - private void confiugreOperatorPadButtons() { + private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new ShooterAdjustWheel(m_shooter, 100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new ShooterAdjustWheel(m_shooter, -100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterSetWheel(m_shooter, 1000)); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 846e4df..6f13434 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -27,6 +27,21 @@ public class Shooter extends SubsystemBase { public Shooter() { configureTurretTalon(); configureWheelTalon(); + configureHoodTalon(); + configureFeederTalon(); + } + + private void configureFeederTalon() { + hoodTalon.changeControlMode(ControlMode.PercentOutput); + } + + private void configureHoodTalon() { + hoodTalon.config_kP(0, 1.0, 0); + hoodTalon.config_kI(0, 0.0, 0); + hoodTalon.config_kD(0, 0.0, 0); + hoodTalon.config_kF(0, 0.0, 0); + hoodTalon.changeControlMode(ControlMode.Position); + hoodTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); } private void configureWheelTalon() { @@ -55,6 +70,8 @@ public void periodic() { private void UpdateDashboard() { SmartDashboard.putNumber("shooter speed", shooterWheelTalon.get()); + SmartDashboard.putNumber("turet pos", turretTalon.get()); + SmartDashboard.putNumber("hood pos", hoodTalon.get()); } public void setTurretPosition(double pos) { From 0cffeded23e06ae8db0fc760dd0f1c4745a0d770 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Wed, 22 Jan 2020 01:56:57 -0500 Subject: [PATCH 025/121] Removing the subsystems from being passed. --- .../commands/ShooterAdjustWheel.java | 19 ++++--------------- .../robot2020/commands/ShooterSetWheel.java | 19 ++++--------------- .../robot2020/subsystems/Shooter.java | 3 ++- 3 files changed, 10 insertions(+), 31 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java index efa0be1..f9dd06f 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java @@ -12,34 +12,23 @@ import edu.wpi.first.wpilibj2.command.CommandBase; public class ShooterAdjustWheel extends CommandBase { - Shooter m_shooter; + double m_adjust; /** * Creates a new ShooterAdjustWheel. */ - public ShooterAdjustWheel(Shooter shooter, double adjust) { + public ShooterAdjustWheel(double adjust) { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(shooter); + addRequirements(RobotContainer.shooter); - m_shooter = shooter; m_adjust = adjust; } // Called when the command is initially scheduled. @Override public void initialize() { - m_shooter.setShooterWheelSpeed(m_shooter.getShooterWheelSpeed() + m_adjust); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { + RobotContainer.shooter.setShooterWheelSpeed(RobotContainer.shooter.getShooterWheelSpeed() + m_adjust); } // Returns true when the command should end. diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java index f30e714..30b2947 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java @@ -7,39 +7,28 @@ package org.mayheminc.robot2020.commands; +import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.CommandBase; public class ShooterSetWheel extends CommandBase { - Shooter m_shooter; double m_adjust; /** * Creates a new ShooterSetWheel. */ - public ShooterSetWheel(Shooter shooter, double adjust) { + public ShooterSetWheel(double adjust) { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(shooter); + addRequirements(RobotContainer.shooter); - m_shooter = shooter; m_adjust = adjust; } // Called when the command is initially scheduled. @Override public void initialize() { - m_shooter.setShooterWheelSpeed(m_adjust); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { + RobotContainer.shooter.setShooterWheelSpeed(m_adjust); } // Returns true when the command should end. diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 6f13434..2367195 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -32,7 +32,7 @@ public Shooter() { } private void configureFeederTalon() { - hoodTalon.changeControlMode(ControlMode.PercentOutput); + feederTalon.changeControlMode(ControlMode.PercentOutput); } private void configureHoodTalon() { @@ -72,6 +72,7 @@ private void UpdateDashboard() { SmartDashboard.putNumber("shooter speed", shooterWheelTalon.get()); SmartDashboard.putNumber("turet pos", turretTalon.get()); SmartDashboard.putNumber("hood pos", hoodTalon.get()); + SmartDashboard.putNumber("feeder speed", feederTalon.get()); } public void setTurretPosition(double pos) { From 04d80d592f825ea2d2291bb5b93c4853d0c6a0a1 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Tue, 21 Jan 2020 21:01:18 -0500 Subject: [PATCH 026/121] making the subsystems public --- .../mayheminc/robot2020/RobotContainer.java | 19 +++++++++-------- .../autonomousroutines/DriveStraight.java | 7 ++++--- .../autonomousroutines/StayStill.java | 2 +- .../robot2020/commands/DriveZeroGyro.java | 21 ++++--------------- 4 files changed, 19 insertions(+), 30 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 11e151d..3c919b8 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -30,14 +30,15 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - public final Climber m_climber = new Climber(); - public final Magazine m_magazine = new Magazine(); - public final Shooter m_shooter = new Shooter(); - public final Drive m_drive = new Drive(); - public final Autonomous m_autonomous = new Autonomous(); + public static final Climber climber = new Climber(); + public static final Magazine magazine = new Magazine(); + public static final Shooter shooter = new Shooter(); + public static final Drive drive = new Drive(); + public static final Autonomous autonomous = new Autonomous(); - private final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(Constants.Joysticks.DRIVER_JOYSTICK); - private final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(Constants.Joysticks.DRIVER_GAMEPAD); + private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick( + Constants.Joysticks.DRIVER_JOYSTICK); + private static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(Constants.Joysticks.DRIVER_GAMEPAD); private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(Constants.Joysticks.OPERATOR_GAMEPAD); private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick( @@ -55,8 +56,8 @@ public RobotContainer() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - autonomousPrograms.push(new StayStill(this.m_drive)); - autonomousPrograms.push(new DriveStraight(this.m_drive)); + autonomousPrograms.push(new StayStill()); + autonomousPrograms.push(new DriveStraight()); m_autonomous.setAutonomousPrograms(autonomousPrograms); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java index 9fac532..bc67aa6 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java @@ -7,6 +7,7 @@ package org.mayheminc.robot2020.autonomousroutines; +import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.DriveStraightOnHeading; import org.mayheminc.robot2020.commands.DriveZeroGyro; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; @@ -18,9 +19,9 @@ public class DriveStraight extends SequentialCommandGroup { /** * Add your docs here. */ - public DriveStraight(Drive drive) { + public DriveStraight() { - addCommands(new DriveZeroGyro(drive)); - addCommands(new DriveStraightOnHeading(drive, 0.5, DistanceUnits.ENCODER_TICKS, 1000, 0)); + addCommands(new DriveZeroGyro()); + addCommands(new DriveStraightOnHeading(0.5, DistanceUnits.ENCODER_TICKS, 1000, 0)); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java index 22663de..d1006a5 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java @@ -15,7 +15,7 @@ */ public class StayStill extends SequentialCommandGroup { - public StayStill(Drive drive) { + public StayStill() { // Perform needed initialization addCommands(new DriveZeroGyro(drive)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java index f0dd43d..9929064 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java @@ -1,5 +1,6 @@ package org.mayheminc.robot2020.commands; +import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.subsystems.Drive; // import org.mayheminc.robot2019.Robot; @@ -10,22 +11,16 @@ * */ public class DriveZeroGyro extends RobotDisabledCommand { - Drive m_drive; - public DriveZeroGyro(Drive drive) { + public DriveZeroGyro() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); - addRequirements(drive); - m_drive = drive; + addRequirements(RobotContainer.drive); } // Called just before this Command runs the first time public void initialize() { - m_drive.zeroHeadingGyro(0.0); - } - - // Called repeatedly when this Command is scheduled to run - public void execute() { + RobotContainer.drive.zeroHeadingGyro(0.0); } // Make this return true when this Command no longer needs to run execute() @@ -33,12 +28,4 @@ public 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() { - } } From 6e8127f5957d07dc99eba1913741ccc75015125b Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Tue, 21 Jan 2020 21:17:51 -0500 Subject: [PATCH 027/121] Update DriveStraightOnHeading.java --- .../robot2020/commands/DriveStraightOnHeading.java | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java index 71216ba..2865aa4 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java @@ -1,5 +1,6 @@ package org.mayheminc.robot2020.commands; +import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.subsystems.Drive; // import org.mayheminc.robot2019.Robot; @@ -16,14 +17,13 @@ public class DriveStraightOnHeading extends CommandBase { double m_targetPower; double m_desiredDisplacement; double m_desiredHeading; - Drive m_drive; public enum DistanceUnits { ENCODER_TICKS, INCHES }; - public DriveStraightOnHeading(Drive drive, double arg_targetSpeed, double arg_distance, double heading) { - this(drive, arg_targetSpeed, DistanceUnits.INCHES, arg_distance, heading); + public DriveStraightOnHeading(double arg_targetSpeed, double arg_distance, double heading) { + this(arg_targetSpeed, DistanceUnits.INCHES, arg_distance, heading); } /** @@ -31,14 +31,12 @@ public DriveStraightOnHeading(Drive drive, double arg_targetSpeed, double arg_di * @param arg_targetPower +/- motor power [-1.0, +1.0] * @param arg_distance Distance in encoder counts */ - public DriveStraightOnHeading(Drive drive, double arg_targetSpeed, DistanceUnits units, double arg_distance, - double heading) { - addRequirements(drive); + public DriveStraightOnHeading(double arg_targetSpeed, DistanceUnits units, double arg_distance, double heading) { + addRequirements(RobotContainer.drive); if (units == DistanceUnits.INCHES) { arg_distance = arg_distance / Drive.DISTANCE_PER_PULSE; } - m_drive = drive; m_targetPower = arg_targetSpeed; m_desiredDisplacement = Math.abs(arg_distance); m_desiredHeading = heading; From 4bd36ac150bdf88c2c0c15c837aa64476017719c Mon Sep 17 00:00:00 2001 From: robertdeml Date: Wed, 22 Jan 2020 02:14:30 -0500 Subject: [PATCH 028/121] Added Turret command. Still removing subsystem passing. --- .../mayheminc/robot2020/RobotContainer.java | 18 ++++----- .../autonomousroutines/StayStill.java | 2 +- .../commands/ShooterAdjustWheel.java | 1 + .../commands/ShooterSetTurretAbs.java | 38 +++++++++++++++++++ .../robot2020/subsystems/Shooter.java | 19 +++++++++- 5 files changed, 67 insertions(+), 11 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretAbs.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 3c919b8..c33a2b9 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -59,7 +59,7 @@ private void configureAutonomousPrograms() { autonomousPrograms.push(new StayStill()); autonomousPrograms.push(new DriveStraight()); - m_autonomous.setAutonomousPrograms(autonomousPrograms); + autonomous.setAutonomousPrograms(autonomousPrograms); } @@ -83,10 +83,10 @@ private void configureDriverStickButtons() { // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); // // adjust auto parameters - DRIVER_STICK.DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(m_autonomous, 1)); - DRIVER_STICK.DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(m_autonomous, -1)); - DRIVER_STICK.DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(m_autonomous, -1)); - DRIVER_STICK.DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(m_autonomous, 1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(autonomous, 1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(autonomous, -1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(autonomous, -1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(autonomous, 1)); // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); @@ -104,9 +104,9 @@ private void configureOperatorStickButtons() { } private void configureOperatorPadButtons() { - OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new ShooterAdjustWheel(m_shooter, 100)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new ShooterAdjustWheel(m_shooter, -100)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterSetWheel(m_shooter, 1000)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new ShooterAdjustWheel(100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new ShooterAdjustWheel(-100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterSetWheel(1000)); } /** @@ -116,6 +116,6 @@ private void configureOperatorPadButtons() { */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - return m_autonomous.getCurrentCommand(); + return autonomous.getCurrentCommand(); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java index d1006a5..74c867a 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java @@ -18,7 +18,7 @@ public class StayStill extends SequentialCommandGroup { public StayStill() { // Perform needed initialization - addCommands(new DriveZeroGyro(drive)); + addCommands(new DriveZeroGyro()); // ALL DONE! } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java index f9dd06f..090b4e4 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java @@ -7,6 +7,7 @@ package org.mayheminc.robot2020.commands; +import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretAbs.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretAbs.java new file mode 100644 index 0000000..eda4f31 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretAbs.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterSetTurretAbs extends CommandBase { + double m_setPoint; + + /** + * Creates a new ShooterSetTurret. + */ + public ShooterSetTurretAbs(double setPoint) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + + m_setPoint = setPoint; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooter.setTurretPositionAbs(m_setPoint); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 2367195..10e59c5 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -75,10 +75,27 @@ private void UpdateDashboard() { SmartDashboard.putNumber("feeder speed", feederTalon.get()); } - public void setTurretPosition(double pos) { + /** + * Set the absolute turret position. + */ + public void setTurretPositionAbs(double pos) { turretTalon.set(ControlMode.Position, pos); } + /** + * Set the relative turret position + * + * @param pos number of encoder ticks to adjust. + */ + public void setTurretPositionRel(double pos) { + turretTalon.set(ControlMode.Position, getTurretPosition() + pos); + } + + /** + * Get the current position of the turret. + * + * @return + */ public double getTurretPosition() { return turretTalon.get(); } From 0d62f142b87fe964153dbf25fc50c33d24ff4c57 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Tue, 21 Jan 2020 22:42:08 -0500 Subject: [PATCH 029/121] asigned buttons --- .../mayheminc/robot2020/RobotContainer.java | 8 +++- .../robot2020/commands/IntakeSetPosition.java | 48 +++++++++++++++++++ .../robot2020/commands/IntakeSetRollers.java | 44 +++++++++++++++++ .../commands/MagazineSetTurntable.java | 43 +++++++++++++++++ 4 files changed, 141 insertions(+), 2 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index af99ce3..11a6e97 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -34,6 +34,7 @@ public class RobotContainer { public static final Magazine magazine = new Magazine(); public static final Shooter shooter = new Shooter(); public static final Drive drive = new Drive(); + public static final Intake intake = new Intake(); public static final Autonomous autonomous = new Autonomous(); private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick( @@ -109,11 +110,14 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new ShooterSetWheel(0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterAdjustWheel(+100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new ShooterSetWheel(1000)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(true)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new ShooterSetFeeder(0.5)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whileHeld(new ShooterSetFeeder(0.5)); - + OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(0.5)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new ShooterSetHood(0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHood(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2)); } /** diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java new file mode 100644 index 0000000..51f2a95 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeSetPosition extends CommandBase { + boolean m_position; + + /** + * Creates a new IntakeSetPosition. + */ + public IntakeSetPosition(boolean position) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + m_position = position; + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.intake.setExtender(m_position); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java new file mode 100644 index 0000000..56b2e0c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeSetRollers extends CommandBase { + double m_speed; + + /** + * Creates a new IntakeSetRollers. + */ + public IntakeSetRollers(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + m_speed = speed; + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.intake.setRollers(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.intake.setRollers(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java new file mode 100644 index 0000000..20c6a26 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class MagazineSetTurntable extends CommandBase { + boolean m_speed; + + /** + * Creates a new MagazineSetTurntable. + */ + public MagazineSetTurntable(boolean b) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.magazine); + m_speed = b; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.magazine.setTurntableSpeed(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.magazine.setTurntableSpeed(false); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} From 7e85d5b18431305630507331521552f219fa76e7 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Wed, 22 Jan 2020 02:19:37 -0500 Subject: [PATCH 030/121] Compiles! --- .../commands/DriveStraightOnHeading.java | 10 ++--- .../commands/ShooterSetTurretRel.java | 38 +++++++++++++++++++ 2 files changed, 43 insertions(+), 5 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretRel.java diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java index 2865aa4..dca08b3 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java @@ -45,21 +45,21 @@ public DriveStraightOnHeading(double arg_targetSpeed, DistanceUnits units, doubl // Called just before this Command runs the first time @Override public void initialize() { - m_drive.saveInitialWheelDistance(); - m_drive.setDesiredHeading(m_desiredHeading); + RobotContainer.drive.saveInitialWheelDistance(); + RobotContainer.drive.setDesiredHeading(m_desiredHeading); // System.out.println("Starting Routine: Drive Straight On Heading"); } // Called repeatedly when this Command is scheduled to run @Override public void execute() { - m_drive.speedRacerDrive(m_targetPower, 0, false); + RobotContainer.drive.speedRacerDrive(m_targetPower, 0, false); } // Make this return true when this Command no longer needs to run execute() @Override public boolean isFinished() { - int displacement = (int) m_drive.getWheelDistance(); + int displacement = (int) RobotContainer.drive.getWheelDistance(); displacement = Math.abs(displacement); // System.out.println("displacement" + displacement); @@ -70,7 +70,7 @@ public boolean isFinished() { // Called once after isFinished returns true @Override public void end(boolean interrupted) { - m_drive.stop(); + RobotContainer.drive.stop(); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretRel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretRel.java new file mode 100644 index 0000000..ff1d925 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretRel.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterSetTurretRel extends CommandBase { + double m_setPoint; + + /** + * Creates a new ShooterSetTurretRel. + */ + public ShooterSetTurretRel(double setPoint) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + + m_setPoint = setPoint; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooter.setTurretPositionRel(m_setPoint); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} From 81d6f19dcc2cb02c2ee016a06c16df484c9432b5 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Wed, 22 Jan 2020 02:50:05 -0500 Subject: [PATCH 031/121] Added tests for shooter. --- .../mayheminc/robot2020/RobotContainer.java | 13 +++- .../autonomousroutines/TestTurret.java | 62 +++++++++++++++++++ .../robot2020/commands/ShooterSetFeeder.java | 38 ++++++++++++ .../robot2020/commands/ShooterSetHood.java | 33 ++++++++++ .../robot2020/commands/ShooterSetWheel.java | 1 - .../mayheminc/robot2020/commands/Wait.java | 34 ++++++++++ .../robot2020/subsystems/Shooter.java | 8 +++ 7 files changed, 185 insertions(+), 4 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetHood.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/Wait.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index c33a2b9..af99ce3 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -58,6 +58,7 @@ private void configureAutonomousPrograms() { autonomousPrograms.push(new StayStill()); autonomousPrograms.push(new DriveStraight()); + autonomousPrograms.push(new TestTurret()); autonomous.setAutonomousPrograms(autonomousPrograms); @@ -104,9 +105,15 @@ private void configureOperatorStickButtons() { } private void configureOperatorPadButtons() { - OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new ShooterAdjustWheel(100)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new ShooterAdjustWheel(-100)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterSetWheel(1000)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new ShooterAdjustWheel(-100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new ShooterSetWheel(0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterAdjustWheel(+100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new ShooterSetWheel(1000)); + + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whileHeld(new ShooterSetFeeder(0.5)); + + OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new ShooterSetHood(0.2)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHood(-0.2)); } /** diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java new file mode 100644 index 0000000..7235da6 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java @@ -0,0 +1,62 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.ShooterSetTurretAbs; +import org.mayheminc.robot2020.commands.ShooterSetTurretRel; +import org.mayheminc.robot2020.commands.Wait; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class TestTurret extends SequentialCommandGroup { + /** + * Creates a new TestTurret. + */ + public TestTurret() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + addRequirements(RobotContainer.shooter); + + addCommands(new ShooterSetTurretAbs(-10)); + addCommands(new Wait(3)); + addCommands(new ShooterSetTurretAbs(0)); + addCommands(new Wait(3)); + + addCommands(new ShooterSetTurretAbs(10)); + addCommands(new Wait(3)); + addCommands(new ShooterSetTurretAbs(0)); + addCommands(new Wait(3)); + + addCommands(new ShooterSetTurretAbs(-20)); + addCommands(new Wait(3)); + addCommands(new ShooterSetTurretAbs(0)); + addCommands(new Wait(3)); + + addCommands(new ShooterSetTurretAbs(20)); + addCommands(new Wait(3)); + addCommands(new ShooterSetTurretAbs(0)); + addCommands(new Wait(3)); + + addCommands(new ShooterSetTurretAbs(-45)); + addCommands(new Wait(3)); + addCommands(new ShooterSetTurretAbs(0)); + addCommands(new Wait(3)); + + addCommands(new ShooterSetTurretAbs(45)); + addCommands(new Wait(3)); + addCommands(new ShooterSetTurretAbs(0)); + addCommands(new Wait(3)); + + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java new file mode 100644 index 0000000..0c6a044 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterSetFeeder extends CommandBase { + double m_speed; + + /** + * Creates a new ShooterSetFeeder. + */ + public ShooterSetFeeder(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + + m_speed = speed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooter.setFeederSpeed(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.shooter.setFeederSpeed(0.0); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHood.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHood.java new file mode 100644 index 0000000..0c44528 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHood.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterSetHood extends InstantCommand { + double m_adjust; + + /** + * Creates a new ShooterSetHood. + */ + public ShooterSetHood(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double pos = RobotContainer.shooter.getHoodPosition(); + RobotContainer.shooter.setHoodPosition(pos + m_adjust); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java index 30b2947..53fd88a 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java @@ -8,7 +8,6 @@ package org.mayheminc.robot2020.commands; import org.mayheminc.robot2020.RobotContainer; -import org.mayheminc.robot2020.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/org/mayheminc/robot2020/commands/Wait.java b/src/main/java/org/mayheminc/robot2020/commands/Wait.java new file mode 100644 index 0000000..ae5d78f --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/Wait.java @@ -0,0 +1,34 @@ +package org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class Wait extends CommandBase { + Timer m_Timer = new Timer(); + double m_endTime; + + public Wait() { + this(0); + } + + public Wait(double endTime) { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + m_endTime = endTime; + } + + // Called just before this Command runs the first time + public void initialize() { + m_Timer.start(); + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + return m_Timer.hasPeriodPassed(m_endTime); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 10e59c5..3984986 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -75,6 +75,10 @@ private void UpdateDashboard() { SmartDashboard.putNumber("feeder speed", feederTalon.get()); } + public void zeroTurretPosition(int pos) { + turretTalon.setPosition(pos); + } + /** * Set the absolute turret position. */ @@ -104,6 +108,10 @@ public void setHoodPosition(double pos) { hoodTalon.set(ControlMode.Position, pos); } + public double getHoodPosition() { + return hoodTalon.get(); + } + public void setFeederSpeed(double pos) { feederTalon.set(ControlMode.PercentOutput, pos); } From 34ba71f4d2f99e6bc7b84718bd99c8640111e4ba Mon Sep 17 00:00:00 2001 From: robertdeml Date: Wed, 22 Jan 2020 03:43:20 -0500 Subject: [PATCH 032/121] Shifting the buttons. --- .../org/mayheminc/robot2020/Constants.java | 12 +- .../mayheminc/robot2020/RobotContainer.java | 10 +- .../java/org/mayheminc/util/Joysticks.java | 8 ++ .../org/mayheminc/util/MayhemOperatorPad.java | 116 +++++++++--------- 4 files changed, 76 insertions(+), 70 deletions(-) create mode 100644 src/main/java/org/mayheminc/util/Joysticks.java diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 741a039..2831808 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -51,10 +51,10 @@ public final class PDP { public static final int DRIVE_LEFT_B = 4; } - public final class Joysticks { - public static final int DRIVER_GAMEPAD = 0; - public static final int DRIVER_JOYSTICK = 1; - public static final int OPERATOR_GAMEPAD = 2; - public static final int OPERATOR_JOYSTICK = 3; - } + // public final class Joysticks { + // public static final int DRIVER_GAMEPAD = 0; + // public static final int DRIVER_JOYSTICK = 1; + // public static final int OPERATOR_GAMEPAD = 2; + // public static final int OPERATOR_JOYSTICK = 3; + // } } diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index af99ce3..ef04e44 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -36,13 +36,11 @@ public class RobotContainer { public static final Drive drive = new Drive(); public static final Autonomous autonomous = new Autonomous(); - private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick( - Constants.Joysticks.DRIVER_JOYSTICK); - private static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(Constants.Joysticks.DRIVER_GAMEPAD); + private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(Joysticks.DRIVER_JOYSTICK); + private static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(Joysticks.DRIVER_GAMEPAD); - private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(Constants.Joysticks.OPERATOR_GAMEPAD); - private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick( - Constants.Joysticks.OPERATOR_JOYSTICK); + private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(Joysticks.OPERATOR_GAMEPAD); + private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(Joysticks.OPERATOR_JOYSTICK); /** * The container for the robot. Contains subsystems, OI devices, and commands. diff --git a/src/main/java/org/mayheminc/util/Joysticks.java b/src/main/java/org/mayheminc/util/Joysticks.java new file mode 100644 index 0000000..73133c5 --- /dev/null +++ b/src/main/java/org/mayheminc/util/Joysticks.java @@ -0,0 +1,8 @@ +package org.mayheminc.util; + +public final class Joysticks { + public static final int DRIVER_GAMEPAD = 0; + public static final int DRIVER_JOYSTICK = 1; + public static final int OPERATOR_GAMEPAD = 2; + public static final int OPERATOR_JOYSTICK = 3; +} diff --git a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java index 5a06bee..6ddc243 100644 --- a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java +++ b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java @@ -6,70 +6,70 @@ public class MayhemOperatorPad { - public Joystick OPERATOR_PAD; - public Button OPERATOR_PAD_BUTTON_ONE; - public Button OPERATOR_PAD_BUTTON_TWO; - public Button OPERATOR_PAD_BUTTON_THREE; - public Button OPERATOR_PAD_BUTTON_FOUR; - public Button OPERATOR_PAD_BUTTON_FIVE; - public Button OPERATOR_PAD_BUTTON_SIX; - public Button OPERATOR_PAD_BUTTON_SEVEN; - public Button OPERATOR_PAD_BUTTON_EIGHT; - public Button OPERATOR_PAD_BUTTON_NINE; - public Button OPERATOR_PAD_BUTTON_TEN; - public Button OPERATOR_PAD_BUTTON_ELEVEN; - public Button OPERATOR_PAD_BUTTON_TWELVE; - public Button FORCE_FIRE_BUTTON; + public final class OPERATOR_PAD_AXIS { + public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; + public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; + public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; + public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; + } - public JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT; - public JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT; - public JoystickPOVButton OPERATOR_PAD_D_PAD_UP; - public JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN; + public final Joystick OPERATOR_PAD = new Joystick(Joysticks.OPERATOR_GAMEPAD); + public final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); + public Button OPERATOR_PAD_BUTTON_TWO; + public Button OPERATOR_PAD_BUTTON_THREE; + public Button OPERATOR_PAD_BUTTON_FOUR; + public Button OPERATOR_PAD_BUTTON_FIVE; + public Button OPERATOR_PAD_BUTTON_SIX; + public Button OPERATOR_PAD_BUTTON_SEVEN; + public Button OPERATOR_PAD_BUTTON_EIGHT; + public Button OPERATOR_PAD_BUTTON_NINE; + public Button OPERATOR_PAD_BUTTON_TEN; + public Button OPERATOR_PAD_BUTTON_ELEVEN; + public Button OPERATOR_PAD_BUTTON_TWELVE; + public Button FORCE_FIRE_BUTTON; - // Operator Control Buttons - public JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP; - public JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN; - public JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP; - public JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN; + public JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT; + public JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT; + public JoystickPOVButton OPERATOR_PAD_D_PAD_UP; + public JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN; - public final class OPERATOR_PAD_AXIS { - public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; - public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; - public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; - public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; - } + // Operator Control Buttons + public JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP; + public JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN; + public JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP; + public JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN; - public MayhemOperatorPad(int port) { + public MayhemOperatorPad(int port) { - OPERATOR_PAD = new Joystick(port); - OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); - OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); - OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); - OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); - OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); - OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); - OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); - OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); - OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); - OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); - OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); - OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); - FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); + // OPERATOR_PAD = new Joystick(Constants.Joysticks.OPERATOR_GAMEPAD); + // OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); + OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); + OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); + OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); + OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); + OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); + OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); + OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); + OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); + OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); + OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); + OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); + FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); - OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); - OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); - OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); - OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); + OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); + OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); + OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); + OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); - // Operator Control Buttons - OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, - JoystickAxisButton.NEGATIVE_ONLY); - OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, - JoystickAxisButton.POSITIVE_ONLY); - OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, - JoystickAxisButton.NEGATIVE_ONLY); - OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + // Operator Control Buttons + OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - } + } } From 378246396f7ef5f63fa19aa275113d1dc6c6d968 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Thu, 23 Jan 2020 19:50:40 -0500 Subject: [PATCH 033/121] Mayhem Joysticks have public final buttons. --- .../mayheminc/robot2020/RobotContainer.java | 8 +- .../org/mayheminc/util/MayhemDriverPad.java | 159 ++++++++---------- .../org/mayheminc/util/MayhemDriverStick.java | 45 ++--- .../org/mayheminc/util/MayhemOperatorPad.java | 79 +++------ .../mayheminc/util/MayhemOperatorStick.java | 5 +- 5 files changed, 115 insertions(+), 181 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 62139a3..6cb33f4 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -37,11 +37,11 @@ public class RobotContainer { public static final Intake intake = new Intake(); public static final Autonomous autonomous = new Autonomous(); - private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(Joysticks.DRIVER_JOYSTICK); - private static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(Joysticks.DRIVER_GAMEPAD); + private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); + private static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); - private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(Joysticks.OPERATOR_GAMEPAD); - private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(Joysticks.OPERATOR_JOYSTICK); + private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); + private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); /** * The container for the robot. Contains subsystems, OI devices, and commands. diff --git a/src/main/java/org/mayheminc/util/MayhemDriverPad.java b/src/main/java/org/mayheminc/util/MayhemDriverPad.java index 5cfd763..b6c593c 100644 --- a/src/main/java/org/mayheminc/util/MayhemDriverPad.java +++ b/src/main/java/org/mayheminc/util/MayhemDriverPad.java @@ -6,100 +6,77 @@ public class MayhemDriverPad { - public final class GAMEPAD_BUTTION { - public static final int GAMEPAD_F310_A_BUTTON = 1; - public static final int GAMEPAD_F310_B_BUTTON = 2; - public static final int GAMEPAD_F310_X_BUTTON = 3; - public static final int GAMEPAD_F310_Y_BUTTON = 4; - public static final int GAMEPAD_F310_LEFT_BUTTON = 5; - public static final int GAMEPAD_F310_RIGHT_BUTTON = 6; - public static final int GAMEPAD_F310_BACK_BUTTON = 7; - public static final int GAMEPAD_F310_START_BUTTON = 8; - public static final int GAMEPAD_F310_LEFT_STICK_BUTTON = 9; - public static final int GAMEPAD_F310_RIGHT_STICK_BUTTON = 10; - } - - public final class GAMEPAD_AXIS { - public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; - public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; - public static final int GAMEPAD_F310_LEFT_TRIGGER = 2; - public static final int GAMEPAD_F310_RIGHT_TRIGGER = 3; - public static final int GAMEPAD_F310_RIGHT_X_AXIS = 4; - public static final int GAMEPAD_F310_RIGHT_Y_AXIS = 5; - } - - public Joystick DRIVER_PAD; - - public Button DRIVER_PAD_BUTTON_FIVE; - public Button DRIVER_PAD_BUTTON_SIX; - public Button DRIVER_PAD_LEFT_STICK_BUTTON; - public Button DRIVER_PAD_RIGHT_STICK_BUTTON; - - public JoystickPOVButton DRIVER_PAD_D_PAD_UP; - public JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT; - public JoystickPOVButton DRIVER_PAD_D_PAD_DOWN; - public JoystickPOVButton DRIVER_PAD_D_PAD_LEFT; - public Button CLIMB_L3_BUTTON_PAIR; - public Button CLIMB_L2_BUTTON_PAIR; - - // Driver Control Modes - public Button TOGGLE_CLOSED_LOOP_MODE_BUTTON; - public Button TOGGLE_FOD_BUTTON; - - // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO - // NOT USE HERE!!! - public Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON; - public JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON; - public JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON; - - public Button DRIVER_PAD_BACK_BUTTON; - public Button DRIVER_PAD_START_BUTTON; - public Button DRIVER_PAD_GREEN_BUTTON; - public Button DRIVER_PAD_RED_BUTTON; - public Button DRIVER_PAD_BLUE_BUTTON; - public Button DRIVER_PAD_YELLOW_BUTTON; - - public MayhemDriverPad(int port) { - - DRIVER_PAD = new Joystick(port); - - DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger - DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger - DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left - // Stick - // Trigger - DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right - // Stick - // Trigger - - DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); - DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); - DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); - DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); - CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, - GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, plus left top trigger - CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, - GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON);// A=Green, plus left top trigger + public final class GAMEPAD_BUTTION { + public static final int GAMEPAD_F310_A_BUTTON = 1; + public static final int GAMEPAD_F310_B_BUTTON = 2; + public static final int GAMEPAD_F310_X_BUTTON = 3; + public static final int GAMEPAD_F310_Y_BUTTON = 4; + public static final int GAMEPAD_F310_LEFT_BUTTON = 5; + public static final int GAMEPAD_F310_RIGHT_BUTTON = 6; + public static final int GAMEPAD_F310_BACK_BUTTON = 7; + public static final int GAMEPAD_F310_START_BUTTON = 8; + public static final int GAMEPAD_F310_LEFT_STICK_BUTTON = 9; + public static final int GAMEPAD_F310_RIGHT_STICK_BUTTON = 10; + } + + public final class GAMEPAD_AXIS { + public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; + public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; + public static final int GAMEPAD_F310_LEFT_TRIGGER = 2; + public static final int GAMEPAD_F310_RIGHT_TRIGGER = 3; + public static final int GAMEPAD_F310_RIGHT_X_AXIS = 4; + public static final int GAMEPAD_F310_RIGHT_Y_AXIS = 5; + } + + public final Joystick DRIVER_PAD = new Joystick(Joysticks.DRIVER_GAMEPAD);; + + public final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger; + public final Button DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger; + public final Button DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left; + public final Button DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right; + + public final JoystickPOVButton DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); + public final JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); + public final JoystickPOVButton DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); + public final JoystickPOVButton DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); + public final Button CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, + // plus + // left + // top + // trigger + public final Button CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON);// A=Green, + // plus + // left + // top + // trigger // Driver Control Modes - TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); - TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); + public final Button TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); + public final Button TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO // NOT USE HERE!!! - DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, - GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); - DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, - GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); - DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, - GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); - - DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); - DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); - DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1); // Green "A" button - DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button - DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button - DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button - - } + public final Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); + public final JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + public final JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + + public final Button DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); + public final Button DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); + + public final Button DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1);; + public final Button DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button + public final Button DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button + public final Button DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button + + public MayhemDriverPad() { + } } \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/MayhemDriverStick.java b/src/main/java/org/mayheminc/util/MayhemDriverStick.java index fd45990..276f9ee 100644 --- a/src/main/java/org/mayheminc/util/MayhemDriverStick.java +++ b/src/main/java/org/mayheminc/util/MayhemDriverStick.java @@ -7,38 +7,25 @@ public class MayhemDriverStick { - public Joystick Joystick; + public final Joystick Joystick = new Joystick(Joysticks.DRIVER_JOYSTICK); - public Button DRIVER_STICK_BUTTON_ONE_DISABLED; - public Button DRIVER_STICK_BUTTON_ONE_ENABLED; - public Button DRIVER_STICK_BUTTON_TWO; - public Button DRIVER_STICK_BUTTON_THREE; - public Button DRIVER_STICK_BUTTON_FOUR; - public Button DRIVER_STICK_BUTTON_FIVE; - public Button DRIVER_STICK_BUTTON_SIX; - public Button DRIVER_STICK_BUTTON_SEVEN; - public Button DRIVER_STICK_BUTTON_EIGHT; - public Button DRIVER_STICK_BUTTON_NINE; - public Button DRIVER_STICK_BUTTON_TEN; - public Button DRIVER_STICK_BUTTON_ELEVEN; + public final Button DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(Joystick, 1); + public final Button DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(Joystick, 1); + public final Button DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(Joystick, 2); + public final Button DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(Joystick, 3); + public final Button DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(Joystick, 4); + public final Button DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(Joystick, 5); + public final Button DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(Joystick, 6); + public final Button DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(Joystick, 7); + public final Button DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(Joystick, 8); + public final Button DRIVER_STICK_BUTTON_NINE = new JoystickButton(Joystick, 9); + public final Button DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(Joystick, 10); + public final Button DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(Joystick, 11); - public int DRIVER_STICK_X_AXIS = 0; - public int DRIVER_STICK_Y_AXIS = 1; + public final int DRIVER_STICK_X_AXIS = 0; + public final int DRIVER_STICK_Y_AXIS = 1; - public MayhemDriverStick(int port) { - this.Joystick = new Joystick(port); + public MayhemDriverStick() { - DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(Joystick, 1); - DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(Joystick, 1); - DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(Joystick, 2); - DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(Joystick, 3); - DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(Joystick, 4); - DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(Joystick, 5); - DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(Joystick, 6); - DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(Joystick, 7); - DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(Joystick, 8); - DRIVER_STICK_BUTTON_NINE = new JoystickButton(Joystick, 9); - DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(Joystick, 10); - DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(Joystick, 11); } } \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java index 6ddc243..5bcdf91 100644 --- a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java +++ b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java @@ -15,61 +15,32 @@ public final class OPERATOR_PAD_AXIS { public final Joystick OPERATOR_PAD = new Joystick(Joysticks.OPERATOR_GAMEPAD); public final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); - public Button OPERATOR_PAD_BUTTON_TWO; - public Button OPERATOR_PAD_BUTTON_THREE; - public Button OPERATOR_PAD_BUTTON_FOUR; - public Button OPERATOR_PAD_BUTTON_FIVE; - public Button OPERATOR_PAD_BUTTON_SIX; - public Button OPERATOR_PAD_BUTTON_SEVEN; - public Button OPERATOR_PAD_BUTTON_EIGHT; - public Button OPERATOR_PAD_BUTTON_NINE; - public Button OPERATOR_PAD_BUTTON_TEN; - public Button OPERATOR_PAD_BUTTON_ELEVEN; - public Button OPERATOR_PAD_BUTTON_TWELVE; - public Button FORCE_FIRE_BUTTON; - - public JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT; - public JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT; - public JoystickPOVButton OPERATOR_PAD_D_PAD_UP; - public JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN; + public final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); + public final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); + public final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); + public final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); + public final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); + public final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); + public final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); + public final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); + public final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); + public final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); + public final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); + public final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); + + public final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); + public final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); + public final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); + public final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); // Operator Control Buttons - public JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP; - public JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN; - public JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP; - public JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN; - - public MayhemOperatorPad(int port) { - - // OPERATOR_PAD = new Joystick(Constants.Joysticks.OPERATOR_GAMEPAD); - // OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); - OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); - OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); - OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); - OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); - OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); - OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); - OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); - OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); - OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); - OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); - OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); - FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); + public final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + public final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + public final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + public final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); - OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); - OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); - OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); - - // Operator Control Buttons - OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - - } } diff --git a/src/main/java/org/mayheminc/util/MayhemOperatorStick.java b/src/main/java/org/mayheminc/util/MayhemOperatorStick.java index fa1a842..7746893 100644 --- a/src/main/java/org/mayheminc/util/MayhemOperatorStick.java +++ b/src/main/java/org/mayheminc/util/MayhemOperatorStick.java @@ -4,9 +4,8 @@ public class MayhemOperatorStick { - public Joystick OPERATOR_STICK; + public final Joystick OPERATOR_STICK = new Joystick(Joysticks.OPERATOR_JOYSTICK); - public MayhemOperatorStick(int port) { - OPERATOR_STICK = new Joystick(port); + public MayhemOperatorStick() { } } \ No newline at end of file From a2063ffecf5cfc5f3e66dedae9b498a91512c4c2 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Thu, 23 Jan 2020 22:04:07 -0500 Subject: [PATCH 034/121] added the drive default command --- .../org/mayheminc/robot2020/Constants.java | 8 ++-- .../mayheminc/robot2020/RobotContainer.java | 7 +++- .../robot2020/commands/DriveDefault.java | 37 ++++++++++++++++++ .../org/mayheminc/util/MayhemDriverPad.java | 39 +++++++++++++++++++ 4 files changed, 86 insertions(+), 5 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 2831808..9a13cc1 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -20,10 +20,10 @@ public final class Constants { public final class Talon { - public static final int DRIVE_RIGHT_A = 1; // high current - public static final int DRIVE_RIGHT_B = 2; // high current - public static final int DRIVE_LEFT_A = 3; // high current - public static final int DRIVE_LEFT_B = 4; // high current + public static final int DRIVE_RIGHT_A = 3; // high current + public static final int DRIVE_RIGHT_B = 4; // high current + public static final int DRIVE_LEFT_A = 1; // high current + public static final int DRIVE_LEFT_B = 2; // high current public static final int SHOOTER_WHEEL = 5; // high current public static final int SHOOTER_FEEDER = 6; diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 6cb33f4..74523a3 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -38,7 +38,7 @@ public class RobotContainer { public static final Autonomous autonomous = new Autonomous(); private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); - private static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); + public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); @@ -50,6 +50,11 @@ public RobotContainer() { // Configure the button bindings configureButtonBindings(); configureAutonomousPrograms(); + configureDefaultCommands(); + } + + private void configureDefaultCommands() { + drive.setDefaultCommand(new DriveDefault()); } private void configureAutonomousPrograms() { diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java b/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java new file mode 100644 index 0000000..c884865 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class DriveDefault extends CommandBase { + /** + * Creates a new DriveDefault. + */ + public DriveDefault() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.drive); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double throttle = RobotContainer.DRIVER_PAD.driveThrottle(); + double steeringX = RobotContainer.DRIVER_PAD.steeringX(); + boolean quickTurn = RobotContainer.DRIVER_PAD.quickTurn(); + RobotContainer.drive.speedRacerDrive(throttle, steeringX, quickTurn); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/util/MayhemDriverPad.java b/src/main/java/org/mayheminc/util/MayhemDriverPad.java index b6c593c..8b40d7a 100644 --- a/src/main/java/org/mayheminc/util/MayhemDriverPad.java +++ b/src/main/java/org/mayheminc/util/MayhemDriverPad.java @@ -79,4 +79,43 @@ public final class GAMEPAD_AXIS { public MayhemDriverPad() { } + + private static final double THROTTLE_DEAD_ZONE_PERCENT = 0.05; + private static final double STEERING_DEAD_ZONE_PERCENT = 0.05; + + public double driveThrottle() { + // the driveThrottle is the "Y" axis of the Driver Gamepad. + // However, the joysticks give -1.0 on the Y axis when pushed forward + // This method reverses that, so that positive numbers are forward + double throttleVal = -DRIVER_PAD.getY(); + + if (Math.abs(throttleVal) < THROTTLE_DEAD_ZONE_PERCENT) { + throttleVal = 0.0; + } + + // if the slow button is pressed, cut the throttle value in third. + // if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { + // throttleVal = throttleVal / 3.0; + // } + + return (throttleVal); + } + + public double steeringX() { + // SteeringX is the "X" axis of the right stick on the Driver Gamepad. + double value = DRIVER_PAD.getRawAxis(GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_X_AXIS); + if (Math.abs(value) < STEERING_DEAD_ZONE_PERCENT) { + value = 0.0; + } + + // if the slow button is pressed, cut the steering value in half. + // if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { + // value = value / 2.0; + // } + return value; + } + + public boolean quickTurn() { + return (DRIVER_PAD.getRawButton(GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_BUTTON)); + } } \ No newline at end of file From 384e08d2974d2c6070e088339ae9c4231548429b Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 25 Jan 2020 16:34:11 -0500 Subject: [PATCH 035/121] First code into the robot Removed example command and subsystem. DriveStraight actually turns to test running the auto command. Cleaned up the unused imports. Changed the Magazine turntable to pass in a speed. Copied the RunAutonomous command. Added the magazine chimney talon. Replaced the MayhemTalonSRX with Fake versions for testing on the 2019 robot. --- build.gradle | 2 +- .../frc/robot/commands/ExampleCommand.java | 51 --------- .../robot/subsystems/ExampleSubsystem.java | 24 ---- .../org/mayheminc/robot2020/Constants.java | 18 +-- .../mayheminc/robot2020/RobotContainer.java | 11 +- .../autonomousroutines/DriveStraight.java | 12 +- .../autonomousroutines/StayStill.java | 1 - .../autonomousroutines/TestTurret.java | 1 - .../robot2020/commands/DriveZeroGyro.java | 1 - .../commands/MagazineSetTurntable.java | 8 +- .../robot2020/commands/RunAutonomous.java | 48 ++++++++ .../commands/ShooterAdjustWheel.java | 1 - .../robot2020/subsystems/Autonomous.java | 22 +++- .../robot2020/subsystems/Climber.java | 15 ++- .../robot2020/subsystems/Intake.java | 9 +- .../robot2020/subsystems/Magazine.java | 32 ++++-- .../robot2020/subsystems/Shooter.java | 22 ++-- .../org/mayheminc/util/MayhemDriverStick.java | 1 - .../mayheminc/util/MayhemFakeTalonSRX.java | 58 ++++++++++ .../org/mayheminc/util/MayhemTalonSRX.java | 106 ++++++++++-------- 20 files changed, 248 insertions(+), 195 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ExampleCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/ExampleSubsystem.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java create mode 100644 src/main/java/org/mayheminc/util/MayhemFakeTalonSRX.java diff --git a/build.gradle b/build.gradle index 0e4fe1a..becc3d0 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.1.2" + id "edu.wpi.first.GradleRIO" version "2020.2.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java deleted file mode 100644 index 42c175f..0000000 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ /dev/null @@ -1,51 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands; - -import frc.robot.subsystems.ExampleSubsystem; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * An example command that uses an example subsystem. - */ -public class ExampleCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ExampleSubsystem m_subsystem; - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ExampleCommand(ExampleSubsystem subsystem) { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index b2585b9..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,24 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ExampleSubsystem extends SubsystemBase { - /** - * Creates a new ExampleSubsystem. - */ - public ExampleSubsystem() { - - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 9a13cc1..b349aa5 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -34,14 +34,14 @@ public final class Talon { public static final int INTAKE_EXTENDER = 10; public static final int MAGAZINE_TURNTABLE = 11; + public static final int MAGAZINE_CHIMNEY = 12; - public static final int CLIMBER_WINCH_LEFT = 12; // high current - public static final int CLIMBER_WINCH_RIGHT = 13; // high current + public static final int CLIMBER_WINCH_LEFT = 13; // high current + public static final int CLIMBER_WINCH_RIGHT = 14; // high current + public static final int CLIMBER_WALKER_LEFT = 15; + public static final int CLIMBER_WALKER_RIGHT = 16; - public static final int CLIMBER_WALKER_LEFT = 14; - public static final int CLIMBER_WALKER_RIGHT = 15; - - public static final int CONTROL_PANEL_ROLLER = 16; + public static final int CONTROL_PANEL_ROLLER = 17; // WON"T FIT!!! } public final class PDP { @@ -51,10 +51,4 @@ public final class PDP { public static final int DRIVE_LEFT_B = 4; } - // public final class Joysticks { - // public static final int DRIVER_GAMEPAD = 0; - // public static final int DRIVER_JOYSTICK = 1; - // public static final int OPERATOR_GAMEPAD = 2; - // public static final int OPERATOR_JOYSTICK = 3; - // } } diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 74523a3..adb564a 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -10,9 +10,7 @@ import org.mayheminc.util.*; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; import java.util.LinkedList; @@ -60,9 +58,9 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - autonomousPrograms.push(new StayStill()); + // autonomousPrograms.push(new StayStill()); autonomousPrograms.push(new DriveStraight()); - autonomousPrograms.push(new TestTurret()); + // autonomousPrograms.push(new TestTurret()); autonomous.setAutonomousPrograms(autonomousPrograms); @@ -113,7 +111,7 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new ShooterSetWheel(0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterAdjustWheel(+100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new ShooterSetWheel(1000)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(true)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(0.3)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new ShooterSetFeeder(0.5)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(0.5)); @@ -129,7 +127,6 @@ private void configureOperatorPadButtons() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous - return autonomous.getCurrentCommand(); + return new RunAutonomous(); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java index bc67aa6..a67454b 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java @@ -7,11 +7,8 @@ package org.mayheminc.robot2020.autonomousroutines; -import org.mayheminc.robot2020.RobotContainer; -import org.mayheminc.robot2020.commands.DriveStraightOnHeading; -import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; -import org.mayheminc.robot2020.subsystems.Drive; import edu.wpi.first.wpilibj2.command.*; @@ -22,6 +19,11 @@ public class DriveStraight extends SequentialCommandGroup { public DriveStraight() { addCommands(new DriveZeroGyro()); - addCommands(new DriveStraightOnHeading(0.5, DistanceUnits.ENCODER_TICKS, 1000, 0)); + addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 10)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 20)); + + // addCommands(new ParallelCommandGroup(new IntakeSetPosition(true), new + // MagazineSetTurntable(0.0))); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java index 74c867a..99476f7 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.*; import org.mayheminc.robot2020.commands.DriveZeroGyro; -import org.mayheminc.robot2020.subsystems.Drive; /** * diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java index 7235da6..b7ae554 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java @@ -9,7 +9,6 @@ import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.ShooterSetTurretAbs; -import org.mayheminc.robot2020.commands.ShooterSetTurretRel; import org.mayheminc.robot2020.commands.Wait; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java index 9929064..53f6cc7 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java @@ -1,7 +1,6 @@ package org.mayheminc.robot2020.commands; import org.mayheminc.robot2020.RobotContainer; -import org.mayheminc.robot2020.subsystems.Drive; // import org.mayheminc.robot2019.Robot; diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java index 20c6a26..a0de4ea 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java @@ -12,15 +12,15 @@ import edu.wpi.first.wpilibj2.command.CommandBase; public class MagazineSetTurntable extends CommandBase { - boolean m_speed; + double m_speed; /** * Creates a new MagazineSetTurntable. */ - public MagazineSetTurntable(boolean b) { + public MagazineSetTurntable(double d) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.magazine); - m_speed = b; + m_speed = d; } // Called when the command is initially scheduled. @@ -32,7 +32,7 @@ public void initialize() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - RobotContainer.magazine.setTurntableSpeed(false); + RobotContainer.magazine.setTurntableSpeed(0.0); } // Returns true when the command should end. diff --git a/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java b/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java new file mode 100644 index 0000000..f67311a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * Delay the set amount of time then schedule the selected command to run. + */ +public class RunAutonomous extends CommandBase { + private long startTime; + Command command; + + /** + * Creates a new RunAutonomous. + */ + public RunAutonomous() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called just before this Command runs the first time + public void initialize() { + startTime = System.currentTimeMillis() + RobotContainer.autonomous.getDelay() * 1000; + command = RobotContainer.autonomous.getSelectedProgram(); + } + + // Called repeatedly when this Command is scheduled to run + public void execute() { + if (System.currentTimeMillis() >= startTime) { + CommandScheduler.getInstance().schedule(command); + } + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + return System.currentTimeMillis() >= startTime; + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java index 090b4e4..c96adfb 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java @@ -8,7 +8,6 @@ package org.mayheminc.robot2020.commands; import org.mayheminc.robot2020.RobotContainer; -import org.mayheminc.robot2020.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java index 3a7d67f..dfafde3 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java @@ -4,11 +4,8 @@ */ package org.mayheminc.robot2020.subsystems; -import java.util.ArrayList; import java.util.LinkedList; - -import org.mayheminc.robot2020.autonomousroutines.*; - +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -26,6 +23,11 @@ public class Autonomous extends SubsystemBase { public Autonomous() { } + /** + * Give the autonomous subsystem the list of possible programs to run + * + * @param programs + */ public void setAutonomousPrograms(LinkedList programs) { autonomousPrograms = programs; } @@ -34,6 +36,9 @@ public Command getSelectedProgram() { return autonomousPrograms.get(programNumber); } + /** + * Returns the delay (0-9). + */ public int getDelay() { return delay; } @@ -60,14 +65,19 @@ public void adjustDelay(final int delta) { updateSmartDashboard(); } + // keep a string buffer in the object so it is not created each iteration. private StringBuffer sb = new StringBuffer(); + /** + * Update the smart dashboard with the auto program and delay. + */ public void updateSmartDashboard() { sb.setLength(0); sb.append(programNumber + " " + autonomousPrograms.get(programNumber).getName()); sb.append(" "); - // SmartDashboard.putString("Auto Prog", sb.toString()); - // SmartDashboard.putNumber("Auto Delay", delay); + + SmartDashboard.putString("Auto Prog", sb.toString()); + SmartDashboard.putNumber("Auto Delay", delay); } public String toString() { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 9bd651c..dbbc53d 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -11,16 +11,17 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import org.mayheminc.robot2020.Constants; - +import org.mayheminc.util.MayhemFakeTalonSRX; import org.mayheminc.util.MayhemTalonSRX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { - private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT); - private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT); - private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); - private final MayhemTalonSRX walkerRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); + private final MayhemFakeTalonSRX winchLeft = new MayhemFakeTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT); + private final MayhemFakeTalonSRX winchRight = new MayhemFakeTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT); + private final MayhemFakeTalonSRX walkerLeft = new MayhemFakeTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); + private final MayhemFakeTalonSRX walkerRight = new MayhemFakeTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); /** * Creates a new Climber. @@ -47,6 +48,10 @@ public Climber() { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Climber Winch Left", winchLeft.get()); + SmartDashboard.putNumber("Climber Winch Right", winchRight.get()); + SmartDashboard.putNumber("Climber Walker Left", walkerLeft.get()); + SmartDashboard.putNumber("Climber Walker Right", walkerRight.get()); } public void setWinchLeftSpeed(double power) { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 8a3bac8..d8925a5 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -11,15 +11,16 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import org.mayheminc.robot2020.Constants; - +import org.mayheminc.util.MayhemFakeTalonSRX; import org.mayheminc.util.MayhemTalonSRX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase { - private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_ROLLERS); - private final MayhemTalonSRX extenderTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_EXTENDER); + private final MayhemFakeTalonSRX rollerTalon = new MayhemFakeTalonSRX(Constants.Talon.INTAKE_ROLLERS); + private final MayhemFakeTalonSRX extenderTalon = new MayhemFakeTalonSRX(Constants.Talon.INTAKE_EXTENDER); /** * Creates a new Intake. @@ -48,5 +49,7 @@ public void setExtender(boolean b) { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Intake Position", extenderTalon.get()); + SmartDashboard.putNumber("Intake Rollers", rollerTalon.get()); } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java index 9bf717d..50efa08 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java @@ -7,38 +7,46 @@ package org.mayheminc.robot2020.subsystems; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.NeutralMode; import org.mayheminc.robot2020.Constants; +import org.mayheminc.util.MayhemFakeTalonSRX; import org.mayheminc.util.MayhemTalonSRX; public class Magazine extends SubsystemBase { - private final MayhemTalonSRX turntableTalon = new MayhemTalonSRX(Constants.Talon.MAGAZINE_TURNTABLE); + private final MayhemFakeTalonSRX turntableTalon = new MayhemFakeTalonSRX(Constants.Talon.MAGAZINE_TURNTABLE); + private final MayhemFakeTalonSRX chimneyTalen = new MayhemFakeTalonSRX(Constants.Talon.MAGAZINE_CHIMNEY); /** * Creates a new Magazine. */ public Magazine() { - turntableTalon.setNeutralMode(NeutralMode.Coast); - turntableTalon.configNominalOutputVoltage(+0.0f, -0.0f); - turntableTalon.configPeakOutputVoltage(+12.0, -12.0); + ConfigureTalon(turntableTalon); + ConfigureTalon(chimneyTalen); + } + + private void ConfigureTalon(MayhemFakeTalonSRX talon) { + talon.setNeutralMode(NeutralMode.Coast); + talon.configNominalOutputVoltage(+0.0f, -0.0f); + talon.configPeakOutputVoltage(+12.0, -12.0); } @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Magazine Turntable", turntableTalon.get()); } - public void setTurntableSpeed(boolean b) { - - if (b) { - turntableTalon.set(1); - } else { - turntableTalon.set(0); - } + public void setTurntableSpeed(double speed) { + turntableTalon.set(ControlMode.PercentOutput, speed); + } - }; + public void setChimneySpeed(double speed) { + chimneyTalen.set(ControlMode.PercentOutput, speed); + } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 3984986..c0d9fd3 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -1,25 +1,21 @@ package org.mayheminc.robot2020.subsystems; -import com.kauailabs.navx.frc.*; - import org.mayheminc.robot2020.Constants; -import org.mayheminc.util.History; -import edu.wpi.first.wpilibj.*; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.mayheminc.util.MayhemFakeTalonSRX; import org.mayheminc.util.MayhemTalonSRX; -import org.mayheminc.util.Utils; public class Shooter extends SubsystemBase { - private final MayhemTalonSRX shooterWheelTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL); - private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); - private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); - private final MayhemTalonSRX feederTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_FEEDER); + private final MayhemFakeTalonSRX shooterWheelTalon = new MayhemFakeTalonSRX(Constants.Talon.SHOOTER_WHEEL); + private final MayhemFakeTalonSRX turretTalon = new MayhemFakeTalonSRX(Constants.Talon.SHOOTER_TURRET); + private final MayhemFakeTalonSRX hoodTalon = new MayhemFakeTalonSRX(Constants.Talon.SHOOTER_HOOD); + private final MayhemFakeTalonSRX feederTalon = new MayhemFakeTalonSRX(Constants.Talon.SHOOTER_FEEDER); /** * Creates a new Shooter. @@ -69,10 +65,10 @@ public void periodic() { } private void UpdateDashboard() { - SmartDashboard.putNumber("shooter speed", shooterWheelTalon.get()); - SmartDashboard.putNumber("turet pos", turretTalon.get()); - SmartDashboard.putNumber("hood pos", hoodTalon.get()); - SmartDashboard.putNumber("feeder speed", feederTalon.get()); + SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelTalon.get()); + SmartDashboard.putNumber("Shooter turet pos", turretTalon.get()); + SmartDashboard.putNumber("Shooter hood pos", hoodTalon.get()); + SmartDashboard.putNumber("Shooter feeder speed", feederTalon.get()); } public void zeroTurretPosition(int pos) { diff --git a/src/main/java/org/mayheminc/util/MayhemDriverStick.java b/src/main/java/org/mayheminc/util/MayhemDriverStick.java index 276f9ee..7230b61 100644 --- a/src/main/java/org/mayheminc/util/MayhemDriverStick.java +++ b/src/main/java/org/mayheminc/util/MayhemDriverStick.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj2.command.button.Button; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; public class MayhemDriverStick { diff --git a/src/main/java/org/mayheminc/util/MayhemFakeTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemFakeTalonSRX.java new file mode 100644 index 0000000..bb7b83b --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemFakeTalonSRX.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.util; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +/** + * Replace a MayhemTalonSRX if you don't have hardware yet. + */ +public class MayhemFakeTalonSRX { + public MayhemFakeTalonSRX(int id) { + + } + + public void setNeutralMode(NeutralMode coast) { + } + + public void configNominalOutputVoltage(float f, float g) { + } + + public void configPeakOutputVoltage(double d, double e) { + } + + public void set(ControlMode percentoutput, double speed) { + } + + public double get() { + return 0; + } + + public void changeControlMode(ControlMode percentoutput) { + } + + public void config_kP(int i, double d, int j) { + } + + public void config_kI(int i, double d, int j) { + } + + public void config_kF(int i, double d, int j) { + } + + public void config_kD(int i, double d, int j) { + } + + public void configSelectedFeedbackSensor(FeedbackDevice quadencoder) { + } + + public void setPosition(int pos) { + } +} diff --git a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java index d605650..027c70d 100644 --- a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java +++ b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -13,92 +13,103 @@ public class MayhemTalonSRX extends TalonSRX { double i; double d; double f; - + public MayhemTalonSRX(int deviceNumber) { super(deviceNumber); - - this.configNominalOutputForward(0.0, 0); - this.configNominalOutputReverse(0.0, 0); - this.configPeakOutputForward(1.0, 0); + + this.configNominalOutputForward(0.0, 0); + this.configNominalOutputReverse(0.0, 0); + this.configPeakOutputForward(1.0, 0); this.configPeakOutputReverse(-1.0, 0); - + this.setNeutralMode(NeutralMode.Coast); -// this.configContinuousCurrentLimit(0, 0); -// this.configPeakCurrentLimit(0, 0); -// this.configPeakCurrentDuration(0, 0); -// this.configForwardLimitSwitchSource(RemoteLimitSwitchSource.Deactivated, LimitSwitchNormal.Disabled, 0, 0); -// this.configForwardSoftLimitEnable(false, 0); + // this.configContinuousCurrentLimit(0, 0); + // this.configPeakCurrentLimit(0, 0); + // this.configPeakCurrentDuration(0, 0); + // this.configForwardLimitSwitchSource(RemoteLimitSwitchSource.Deactivated, + // LimitSwitchNormal.Disabled, 0, 0); + // this.configForwardSoftLimitEnable(false, 0); - // copied from CTRE Example: https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java/Current%20Limit/src/org/usfirst/frc/team217/robot/Robot.java#L37 -// this.configPeakCurrentLimit(80, 10); -// this.configPeakCurrentDuration(60000, 10); /* this is a necessary call to avoid errata. */ -// this.configContinuousCurrentLimit(40, 10); -// this.enableCurrentLimit(true); /* honor initial setting */ + // copied from CTRE Example: + // https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java/Current%20Limit/src/org/usfirst/frc/team217/robot/Robot.java#L37 + // this.configPeakCurrentLimit(80, 10); + // this.configPeakCurrentDuration(60000, 10); /* this is a necessary call to + // avoid errata. */ + // this.configContinuousCurrentLimit(40, 10); + // this.enableCurrentLimit(true); /* honor initial setting */ } - public ErrorCode config_kP(int slot, double value, int timeout) - { + public ErrorCode config_kP(int slot, double value, int timeout) { p = value; return super.config_kP(slot, value, timeout); } - - public ErrorCode config_kI(int slot, double value, int timeout) - { + + public ErrorCode config_kI(int slot, double value, int timeout) { i = value; return super.config_kI(slot, value, timeout); } - - public ErrorCode config_kD(int slot, double value, int timeout) - { + + public ErrorCode config_kD(int slot, double value, int timeout) { d = value; return super.config_kD(slot, value, timeout); } - public ErrorCode config_kF(int slot, double value, int timeout) - { + + public ErrorCode config_kF(int slot, double value, int timeout) { f = value; return super.config_kF(slot, value, timeout); } - - public double getP() {return p;} - public double getI() {return i;} - public double getD() {return d;} - public double getF() {return f;} - public void changeControlMode(ControlMode mode) { + public double getP() { + return p; + } + + public double getI() { + return i; + } + + public double getD() { + return d; + } + + public double getF() { + return f; + } + + public void changeControlMode(ControlMode mode) { controlMode = mode; } public void set(int deviceID) { this.set(controlMode, deviceID); } - + public void setFeedbackDevice(FeedbackDevice feedback) { this.configSelectedFeedbackSensor(feedback, 0, 1000); } public void reverseSensor(boolean b) { - + } public void configNominalOutputVoltage(float f, float g) { - this.configNominalOutputForward(f/12.0, 1000); - this.configNominalOutputReverse(g/12.0, 1000); + this.configNominalOutputForward(f / 12.0, 1000); + this.configNominalOutputReverse(g / 12.0, 1000); } public void configPeakOutputVoltage(double d, double e) { - this.configPeakOutputForward(d/12.0, 1000); - this.configPeakOutputReverse(e/12.0, 1000); + this.configPeakOutputForward(d / 12.0, 1000); + this.configPeakOutputReverse(e / 12.0, 1000); } public void setPID(double wheelP, double wheelI, double wheelD, double wheelF, int i, double m_voltageRampRate, int j) { - this.config_kP(pidProfile, wheelP , 1000); - this.config_kI(pidProfile, wheelI , 1000); - this.config_kD(pidProfile, wheelD , 1000); - this.config_kF(pidProfile, wheelF , 1000); + this.config_kP(pidProfile, wheelP, 1000); + this.config_kI(pidProfile, wheelI, 1000); + this.config_kD(pidProfile, wheelD, 1000); + this.config_kF(pidProfile, wheelF, 1000); } public double getSetpoint() { @@ -110,16 +121,17 @@ public double getError() { } public float getOutputVoltage() { - return (float) this.getMotorOutputVoltage(); + return (float) this.getMotorOutputVoltage(); } int pidProfile; + public void setProfile(int pidSlot) { - pidProfile = pidSlot; + pidProfile = pidSlot; } public void setPID(double pDown, double iDown, double dDown) { - setPID(pDown, iDown, dDown, 0.0, 0, 0.0, 0); + setPID(pDown, iDown, dDown, 0.0, 0, 0.0, 0); } public void setVoltageRampRate(double d) { @@ -128,11 +140,11 @@ public void setVoltageRampRate(double d) { } public void enableControl() { - + } public void setPosition(int zeroPositionCount) { - this.setSelectedSensorPosition(zeroPositionCount, 0, 1000); + this.setSelectedSensorPosition(zeroPositionCount, 0, 1000); } public int getPosition() { From 0493b0770d67cdc7c079daa90c12cf43e4efbf91 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Mon, 27 Jan 2020 20:14:06 -0500 Subject: [PATCH 036/121] fixed bug in autonomus added set chimney --- .../commands/MagazineSetChimney.java | 43 +++++++++++++++++++ .../robot2020/commands/RunAutonomous.java | 4 +- 2 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java new file mode 100644 index 0000000..bacf111 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class MagazineSetChimney extends CommandBase { + /** + * Creates a new MagazineSetChimney. + */ + public MagazineSetChimney() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.magazine); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java b/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java index f67311a..c2a7826 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java +++ b/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java @@ -19,6 +19,7 @@ public class RunAutonomous extends CommandBase { private long startTime; Command command; + boolean commandIsRunning; /** * Creates a new RunAutonomous. @@ -37,12 +38,13 @@ public void initialize() { public void execute() { if (System.currentTimeMillis() >= startTime) { CommandScheduler.getInstance().schedule(command); + commandIsRunning = true; } } // Make this return true when this Command no longer needs to run execute() public boolean isFinished() { - return System.currentTimeMillis() >= startTime; + return commandIsRunning; } } \ No newline at end of file From 4a41dea7d4738870ddafe395c776f9ae6c688c86 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Mon, 27 Jan 2020 21:50:05 -0500 Subject: [PATCH 037/121] added a readyAimFire --- .../mayheminc/robot2020/RobotContainer.java | 1 + .../commands/MagazineSetChimney.java | 13 +++-- .../commands/ShooterFireWhenReady.java | 50 ++++++++++++++++++ .../commands/ShooterReadyAimFire.java | 32 ++++++++++++ .../commands/TargetingIsOnTarget.java | 51 +++++++++++++++++++ .../robot2020/subsystems/Targeting.java | 40 +++++++++++++++ 6 files changed, 180 insertions(+), 7 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index adb564a..49db7c3 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -34,6 +34,7 @@ public class RobotContainer { public static final Drive drive = new Drive(); public static final Intake intake = new Intake(); public static final Autonomous autonomous = new Autonomous(); + public static final Targeting targeting = new Targeting(); private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java index bacf111..cfd2834 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java @@ -12,22 +12,21 @@ import edu.wpi.first.wpilibj2.command.CommandBase; public class MagazineSetChimney extends CommandBase { + double m_speed; + /** * Creates a new MagazineSetChimney. */ - public MagazineSetChimney() { + public MagazineSetChimney(double d) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.magazine); + m_speed = d; } // Called when the command is initially scheduled. @Override public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { + RobotContainer.magazine.setChimneySpeed(m_speed); } // Called once the command ends or is interrupted. @@ -38,6 +37,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java new file mode 100644 index 0000000..61eb2ac --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.subsystems.Targeting; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterFireWhenReady extends CommandBase { + /** + * Creates a new ShooterFireWhenReady. + */ + public ShooterFireWhenReady() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + boolean wheelsGood = Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget()) + - RobotContainer.shooter.getShooterWheelSpeed()) < 100; + + RobotContainer.shooter.setFeederSpeed((wheelsGood) ? 0.5 : 0.0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.shooter.setFeederSpeed(0); + RobotContainer.shooter.setShooterWheelSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java new file mode 100644 index 0000000..ef0ff1b --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShooterReadyAimFire extends SequentialCommandGroup { + /** + * Creates a new ShooterReadyAimFire. + */ + public ShooterReadyAimFire() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + addCommands(new TargetingIsOnTarget()); + addCommands(new MagazineSetTurntable(0.3)); + addCommands(new MagazineSetChimney(0.3)); + addCommands(new ParallelRaceGroup(new ShooterFireWhenReady(), new Wait(5.0))); + addCommands(new MagazineSetTurntable(0.0)); + addCommands(new MagazineSetChimney(0.0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java new file mode 100644 index 0000000..3c88df0 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.subsystems.Targeting; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TargetingIsOnTarget extends CommandBase { + /** + * Creates a new TargetingIsOnTarget. + */ + public TargetingIsOnTarget() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double bearingToTarget = RobotContainer.targeting.getBearingToTarget(); + double rangeToTarget = RobotContainer.targeting.getRangeToTarget(); + + RobotContainer.shooter.setTurretPositionRel(RobotContainer.shooter.getTurretPosition() + bearingToTarget); + RobotContainer.shooter.setShooterWheelSpeed(Targeting.convertRangeToWheelSpeed(rangeToTarget)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + boolean bearingGood = Math.abs(RobotContainer.targeting.getBearingToTarget()) < 2; + boolean wheelsGood = Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget()) + - RobotContainer.shooter.getShooterWheelSpeed()) < 100; + return bearingGood && wheelsGood; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java new file mode 100644 index 0000000..b16c645 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Targeting extends SubsystemBase { + /** + * Creates a new Targeting. + */ + public Targeting() { + + } + + public boolean isOnTarget() { + return true; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + public double getBearingToTarget() { + return 0; + } + + public double getRangeToTarget() { + return 0; + } + + public static double convertRangeToWheelSpeed(double rangeToTarget) { + return 1000; + } +} From cdf8e62614f2d7981c015e9721411649a16d44a9 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Mon, 27 Jan 2020 22:08:44 -0500 Subject: [PATCH 038/121] config intake talon --- .../java/org/mayheminc/robot2020/subsystems/Intake.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index d8925a5..5f16b69 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -8,6 +8,7 @@ package org.mayheminc.robot2020.subsystems; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import org.mayheminc.robot2020.Constants; @@ -20,7 +21,7 @@ public class Intake extends SubsystemBase { private final MayhemFakeTalonSRX rollerTalon = new MayhemFakeTalonSRX(Constants.Talon.INTAKE_ROLLERS); - private final MayhemFakeTalonSRX extenderTalon = new MayhemFakeTalonSRX(Constants.Talon.INTAKE_EXTENDER); + private final MayhemTalonSRX extenderTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_EXTENDER); /** * Creates a new Intake. @@ -30,6 +31,12 @@ public Intake() { rollerTalon.configNominalOutputVoltage(+0.0f, -0.0f); rollerTalon.configPeakOutputVoltage(+12.0, -12.0); + extenderTalon.configNominalOutputVoltage(+0.0f, -0.0f); + extenderTalon.configPeakOutputVoltage(+12.0, -12.0); + extenderTalon.setFeedbackDevice(FeedbackDevice.QuadEncoder); + extenderTalon.config_kP(0, 1.0, 0); + extenderTalon.config_kI(0, 0.0, 0); + extenderTalon.config_kD(0, 0.0, 0); } public void setRollers(double power) { From 66259b60744c2025bc33b7b5f9cabe0b1e98c2a1 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Mon, 3 Feb 2020 21:49:24 -0500 Subject: [PATCH 039/121] tested the intake rollers and pivot. --- .../mayheminc/robot2020/RobotContainer.java | 10 +++++-- .../commands/IntakeExtenderVBus.java | 30 +++++++++++++++++++ .../robot2020/commands/IntakeSetPosition.java | 4 +-- .../robot2020/subsystems/Intake.java | 24 ++++++++++----- .../org/mayheminc/util/MayhemOperatorPad.java | 20 +++++++++++++ 5 files changed, 76 insertions(+), 12 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 49db7c3..8a22bfc 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -39,7 +39,7 @@ public class RobotContainer { private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); - private final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); + public static final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); /** @@ -54,6 +54,7 @@ public RobotContainer() { private void configureDefaultCommands() { drive.setDefaultCommand(new DriveDefault()); + intake.setDefaultCommand(new IntakeExtenderVBus()); } private void configureAutonomousPrograms() { @@ -115,11 +116,16 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(0.3)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new ShooterSetFeeder(0.5)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(0.5)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(-0.5)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new ShooterSetHood(0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHood(-0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2)); + + // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whenPressed(new + // IntakeSetPosition(0.0)); + // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whenPressed(new + // IntakeSetPosition(90.0)); } /** diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java new file mode 100644 index 0000000..959ecb4 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeExtenderVBus extends CommandBase { + /** + * Creates a new IntakeExtenderVBus. + */ + public IntakeExtenderVBus() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double power = RobotContainer.OPERATOR_PAD.getLeftYAxis(); + RobotContainer.intake.setExtenderVBus(power); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java index 51f2a95..52231bd 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java @@ -12,12 +12,12 @@ import edu.wpi.first.wpilibj2.command.CommandBase; public class IntakeSetPosition extends CommandBase { - boolean m_position; + double m_position; /** * Creates a new IntakeSetPosition. */ - public IntakeSetPosition(boolean position) { + public IntakeSetPosition(Double position) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.intake); m_position = position; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 5f16b69..7f8068e 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -20,7 +20,7 @@ public class Intake extends SubsystemBase { - private final MayhemFakeTalonSRX rollerTalon = new MayhemFakeTalonSRX(Constants.Talon.INTAKE_ROLLERS); + private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_ROLLERS); private final MayhemTalonSRX extenderTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_EXTENDER); /** @@ -39,24 +39,32 @@ public Intake() { extenderTalon.config_kD(0, 0.0, 0); } + /** + * Negative for intake. Positive for spit. + * + * @param power + */ public void setRollers(double power) { rollerTalon.set(ControlMode.PercentOutput, power); } - public void setExtender(boolean b) { - if (b) { - extenderTalon.set(ControlMode.Position, 90); - - } else { - extenderTalon.set(ControlMode.Position, 10); - } + public void setExtender(Double b) { + extenderTalon.set(ControlMode.Position, b); } @Override public void periodic() { // This method will be called once per scheduler run + updateSmartDashBoard(); + } + + public void updateSmartDashBoard() { SmartDashboard.putNumber("Intake Position", extenderTalon.get()); SmartDashboard.putNumber("Intake Rollers", rollerTalon.get()); } + + public void setExtenderVBus(double VBus) { + extenderTalon.set(ControlMode.PercentOutput, VBus); + } } diff --git a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java index 5bcdf91..60835a0 100644 --- a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java +++ b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java @@ -43,4 +43,24 @@ public final class OPERATOR_PAD_AXIS { public final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; + public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; + public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; + public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; + + private static final double Y_AXIS_DEAD_ZONE_PERCENT = 0.15; + + public double getLeftYAxis() { + // SteeringX is the "X" axis of the right stick on the Driver Gamepad. + double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_LEFT_Y_AXIS); + if (Math.abs(value) < Y_AXIS_DEAD_ZONE_PERCENT) { + value = 0.0; + } + + // if the slow button is pressed, cut the steering value in half. + // if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { + // value = value / 2.0; + // } + return value; + } } From a3b5382371eb0e812c1835850f8a3f350b1f4e69 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Mon, 3 Feb 2020 23:10:44 -0500 Subject: [PATCH 040/121] Fixed drive base talon direction, turned off "heading correction" in drive (temporarily) until upside-down roboRIO can be debugged, and changed intake speeds to 70% of max. --- .../mayheminc/robot2020/RobotContainer.java | 4 +-- .../mayheminc/robot2020/subsystems/Drive.java | 13 ++++---- .../org/mayheminc/util/MayhemDriverPad.java | 12 ++++---- vendordeps/Phoenix.json | 30 +++++++++---------- vendordeps/navx_frc.json | 9 +++--- 5 files changed, 36 insertions(+), 32 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 8a22bfc..fb7905a 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -114,9 +114,9 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterAdjustWheel(+100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new ShooterSetWheel(1000)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(0.3)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new ShooterSetFeeder(0.5)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-0.7)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(-0.5)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(0.7)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new ShooterSetHood(0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHood(-0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index cef217c..31f1e82 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -114,10 +114,10 @@ public Drive() { // the left motors move the robot forwards with positive power // but the right motors are backwards. - leftFrontTalon.setInverted(true); - leftRearTalon.setInverted(true); - rightFrontTalon.setInverted(false); - rightRearTalon.setInverted(false); + leftFrontTalon.setInverted(false); + leftRearTalon.setInverted(false); + rightFrontTalon.setInverted(true); + rightRearTalon.setInverted(true); // sensor phase is reversed, since there are 3 reduction stages in the gearbox leftFrontTalon.setSensorPhase(true); @@ -440,7 +440,10 @@ public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickT // not using camera targeting right now // check for if steering input is essentially zero - if ((-0.01 < rawSteeringX) && (rawSteeringX < 0.01)) { + if (false /* + * turn off heading correction for now (-0.01 < rawSteeringX) && (rawSteeringX < + * 0.01) + */) { // no turn being commanded, drive straight or stay still m_iterationsSinceRotationCommanded++; if ((-0.01 < throttle) && (throttle < 0.01)) { diff --git a/src/main/java/org/mayheminc/util/MayhemDriverPad.java b/src/main/java/org/mayheminc/util/MayhemDriverPad.java index 8b40d7a..999c3a7 100644 --- a/src/main/java/org/mayheminc/util/MayhemDriverPad.java +++ b/src/main/java/org/mayheminc/util/MayhemDriverPad.java @@ -94,9 +94,9 @@ public double driveThrottle() { } // if the slow button is pressed, cut the throttle value in third. - // if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { - // throttleVal = throttleVal / 3.0; - // } + if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { + throttleVal = throttleVal / 3.0; + } return (throttleVal); } @@ -109,9 +109,9 @@ public double steeringX() { } // if the slow button is pressed, cut the steering value in half. - // if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { - // value = value / 2.0; - // } + if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { + value = value / 2.0; + } return value; } diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index a633555..8b25850 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.17.4", + "version": "5.17.6", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.17.4" + "version": "5.17.6" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.17.4" + "version": "5.17.6" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.4", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.4", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.4", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.4", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -69,7 +69,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.4", + "version": "5.17.6", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.17.4", + "version": "5.17.6", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -97,7 +97,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.17.4", + "version": "5.17.6", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -111,7 +111,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.4", + "version": "5.17.6", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -125,7 +125,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.4", + "version": "5.17.6", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -139,7 +139,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.4", + "version": "5.17.6", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.4", + "version": "5.17.6", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -165,7 +165,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.4", + "version": "5.17.6", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json index 82810d3..f7c0fa4 100644 --- a/vendordeps/navx_frc.json +++ b/vendordeps/navx_frc.json @@ -1,7 +1,7 @@ { "fileName": "navx_frc.json", "name": "KauaiLabs_navX_FRC", - "version": "3.1.400", + "version": "3.1.405", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "mavenUrls": [ "https://repo1.maven.org/maven2/" @@ -11,7 +11,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-java", - "version": "3.1.400" + "version": "3.1.405" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-cpp", - "version": "3.1.400", + "version": "3.1.405", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, @@ -27,7 +27,8 @@ "skipInvalidPlatforms": true, "binaryPlatforms": [ "linuxathena", - "linuxraspbian" + "linuxraspbian", + "windowsx86-x64" ] } ] From dcd70974c4b9e9575387379b0da1c8e323677b3a Mon Sep 17 00:00:00 2001 From: robertdeml Date: Tue, 4 Feb 2020 18:59:04 -0500 Subject: [PATCH 041/121] added a relax to the intake pivot. Added the drive periodic. --- .../mayheminc/robot2020/subsystems/Drive.java | 5 +++++ .../robot2020/subsystems/Intake.java | 22 ++++++++++++++++++- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 31f1e82..62e900b 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -569,6 +569,11 @@ public void rotateToHeading(double desiredHeading) { // **********************************************DISPLAY**************************************************** + @Override + public void periodic(){ + updateSmartDashboard(); + } + public void updateSmartDashboard() { displayGyroInfo(); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 7f8068e..440a5b4 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -12,7 +12,6 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import org.mayheminc.robot2020.Constants; -import org.mayheminc.util.MayhemFakeTalonSRX; import org.mayheminc.util.MayhemTalonSRX; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -20,9 +19,16 @@ public class Intake extends SubsystemBase { + private final int PIVOT_CLOSE_ENOUGH = 100; private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_ROLLERS); private final MayhemTalonSRX extenderTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_EXTENDER); + enum PivotMode { + MANUAL_MODE, PID_MODE, + }; + + PivotMode mode; + /** * Creates a new Intake. */ @@ -51,11 +57,24 @@ public void setRollers(double power) { public void setExtender(Double b) { extenderTalon.set(ControlMode.Position, b); + mode = PivotMode.PID_MODE; } @Override public void periodic() { // This method will be called once per scheduler run + + switch (mode) { + case PID_MODE: + // if the pivot is close enough, turn off the motor + if (Math.abs(extenderTalon.getSetpoint() - extenderTalon.get()) < PIVOT_CLOSE_ENOUGH) { + extenderTalon.set(ControlMode.PercentOutput, 0); + } + break; + default: + break; + } + updateSmartDashBoard(); } @@ -66,5 +85,6 @@ public void updateSmartDashBoard() { public void setExtenderVBus(double VBus) { extenderTalon.set(ControlMode.PercentOutput, VBus); + mode = PivotMode.MANUAL_MODE; } } From bffb739cf424767e5df4ba0b0506b6a0d9c8c68e Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Tue, 4 Feb 2020 22:31:21 -0500 Subject: [PATCH 042/121] Intake and drive updates Intake vbus is squared. Slowed the turns. increased the roller speed. --- .../mayheminc/robot2020/RobotContainer.java | 9 ++-- .../commands/IntakeExtenderVBus.java | 2 + .../robot2020/commands/IntakeSetPosition.java | 12 +---- .../robot2020/commands/IntakeZero.java | 29 ++++++++++++ .../mayheminc/robot2020/subsystems/Drive.java | 7 +-- .../robot2020/subsystems/Intake.java | 46 +++++++++++++++++-- 6 files changed, 83 insertions(+), 22 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/IntakeZero.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index fb7905a..426768a 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -114,11 +114,12 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterAdjustWheel(+100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new ShooterSetWheel(1000)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(0.3)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-0.7)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(0.7)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new ShooterSetHood(0.2)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHood(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN + .whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java index 959ecb4..523c757 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java @@ -24,6 +24,8 @@ public IntakeExtenderVBus() { @Override public void execute() { double power = RobotContainer.OPERATOR_PAD.getLeftYAxis(); + double sign = (power >= 0) ? -1.0 : 1.0;// invert the sign + power = sign * power * power; RobotContainer.intake.setExtenderVBus(power); } diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java index 52231bd..0b07e58 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java @@ -30,19 +30,9 @@ public void initialize() { RobotContainer.intake.setExtender(m_position); } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeZero.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeZero.java new file mode 100644 index 0000000..78ea1c3 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeZero.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class IntakeZero extends InstantCommand { + /** + * Creates a new IntakeZero. + */ + public IntakeZero() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.intake.zero(); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 62e900b..84a2c8b 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -418,8 +418,9 @@ public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickT double leftPower, rightPower; double rotation = 0; double adjustedSteeringX = rawSteeringX * throttle; - final double QUICK_TURN_GAIN = 0.75; // culver drive used 1.5 - final double STD_TURN_GAIN = 1.5; // the driver wants the non-quick turn turning a little more responsive. + final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast. + final double STD_TURN_GAIN = 1.0; // 2019: 1.5. 2020: 1.5 was too fast// the driver wants the non-quick turn + // turning a little more responsive. int throttleSign; if (throttle >= 0.0) { @@ -570,7 +571,7 @@ public void rotateToHeading(double desiredHeading) { // **********************************************DISPLAY**************************************************** @Override - public void periodic(){ + public void periodic() { updateSmartDashboard(); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 440a5b4..cd49258 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -22,12 +22,15 @@ public class Intake extends SubsystemBase { private final int PIVOT_CLOSE_ENOUGH = 100; private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_ROLLERS); private final MayhemTalonSRX extenderTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_EXTENDER); + private final int PIVOT_ZERO_POSITION = 0; + public final double PIVOT_UP = 0.0; + public final double PIVOT_DOWN = 900.0; enum PivotMode { MANUAL_MODE, PID_MODE, }; - PivotMode mode; + PivotMode mode = PivotMode.MANUAL_MODE; /** * Creates a new Intake. @@ -40,11 +43,46 @@ public Intake() { extenderTalon.configNominalOutputVoltage(+0.0f, -0.0f); extenderTalon.configPeakOutputVoltage(+12.0, -12.0); extenderTalon.setFeedbackDevice(FeedbackDevice.QuadEncoder); - extenderTalon.config_kP(0, 1.0, 0); + extenderTalon.config_kP(0, 10.0, 0); extenderTalon.config_kI(0, 0.0, 0); extenderTalon.config_kD(0, 0.0, 0); } + void configMotor(MayhemTalonSRX motor) { + // initial calcs for computing kP... + // If we want 100% power when at the full extreme, + // Full extreme is 900 ticks + // kP = (1.0 * 1023) / 900 = + motor.config_kP(0, 1.15, 0); // based upon Ken's initial calcs, above + + // typical value of about 1/100 of kP for starting tuning + motor.config_kI(0, 0.0, 0); + + // typical value of about 10x to 100x of kP for starting tuning + motor.config_kD(0, 0.0, 0); + // motor.config_kD(0, 0.575, 0); + + // practically always set kF to 0 for position control + // for things like gravity compensation, use the "arbitrary feed forward" that + // can be specified with the "4-parameter" TalonSRX.set() method + motor.config_kF(0, 0.0, 0); + + motor.setNeutralMode(NeutralMode.Coast); + motor.setInverted(false); + motor.setSensorPhase(false); + motor.configNominalOutputVoltage(+0.0f, -0.0f); + motor.configPeakOutputVoltage(+12.0, -12.0); + motor.configClosedloopRamp(0.05); // limit neutral to full to 0.05 seconds + motor.configMotionCruiseVelocity(300); // estimate 300 ticks max + motor.configMotionAcceleration(600); // acceleration of 2x velocity allows cruise to be attained in 1/2 + // second + motor.setFeedbackDevice(FeedbackDevice.QuadEncoder); + } + + public void zero() { + extenderTalon.setEncPosition(PIVOT_ZERO_POSITION); + } + /** * Negative for intake. Positive for spit. * @@ -68,7 +106,7 @@ public void periodic() { case PID_MODE: // if the pivot is close enough, turn off the motor if (Math.abs(extenderTalon.getSetpoint() - extenderTalon.get()) < PIVOT_CLOSE_ENOUGH) { - extenderTalon.set(ControlMode.PercentOutput, 0); + setExtenderVBus(0); } break; default: @@ -79,7 +117,7 @@ public void periodic() { } public void updateSmartDashBoard() { - SmartDashboard.putNumber("Intake Position", extenderTalon.get()); + SmartDashboard.putNumber("Intake Position", extenderTalon.getPosition()); SmartDashboard.putNumber("Intake Rollers", rollerTalon.get()); } From 0f9b60c30975614220e46d5ec2a7d6483a54b5ed Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Thu, 6 Feb 2020 22:32:54 -0500 Subject: [PATCH 043/121] working on buttons for moving the intake up and down --- .../mayheminc/robot2020/RobotContainer.java | 15 ++++-- .../robot2020/commands/IntakeSetPosition.java | 2 +- .../robot2020/subsystems/Intake.java | 54 ++++++++++--------- .../org/mayheminc/util/MayhemTalonSRX.java | 4 +- 4 files changed, 45 insertions(+), 30 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 426768a..81d6244 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -109,10 +109,17 @@ private void configureOperatorStickButtons() { } private void configureOperatorPadButtons() { - OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new ShooterAdjustWheel(-100)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new ShooterSetWheel(0)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new ShooterAdjustWheel(+100)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new ShooterSetWheel(1000)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new IntakeZero()); + // new ShooterAdjustWheel(-100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO + .whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + // new ShooterSetWheel(0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE + .whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + // new ShooterAdjustWheel(+100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR + .whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); + // new ShooterSetWheel(1000)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(0.3)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java index 0b07e58..2c1911a 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java @@ -33,6 +33,6 @@ public void initialize() { // Returns true when the command should end. @Override public boolean isFinished() { - return true; + return RobotContainer.intake.isExtenderAtPosition(); } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index cd49258..bb2fef4 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -19,18 +19,20 @@ public class Intake extends SubsystemBase { - private final int PIVOT_CLOSE_ENOUGH = 100; + private final int PIVOT_CLOSE_ENOUGH = 20; private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_ROLLERS); private final MayhemTalonSRX extenderTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_EXTENDER); - private final int PIVOT_ZERO_POSITION = 0; - public final double PIVOT_UP = 0.0; - public final double PIVOT_DOWN = 900.0; + private final int PIVOT_ZERO_POSITION = 900; + public final double PIVOT_UP = 900.0; + public final double PIVOT_DOWN = 0.0; enum PivotMode { MANUAL_MODE, PID_MODE, }; PivotMode mode = PivotMode.MANUAL_MODE; + boolean isMoving; + double m_targetPosition; /** * Creates a new Intake. @@ -40,26 +42,21 @@ public Intake() { rollerTalon.configNominalOutputVoltage(+0.0f, -0.0f); rollerTalon.configPeakOutputVoltage(+12.0, -12.0); - extenderTalon.configNominalOutputVoltage(+0.0f, -0.0f); - extenderTalon.configPeakOutputVoltage(+12.0, -12.0); - extenderTalon.setFeedbackDevice(FeedbackDevice.QuadEncoder); - extenderTalon.config_kP(0, 10.0, 0); - extenderTalon.config_kI(0, 0.0, 0); - extenderTalon.config_kD(0, 0.0, 0); + configMotor(extenderTalon); } void configMotor(MayhemTalonSRX motor) { // initial calcs for computing kP... - // If we want 100% power when at the full extreme, + // If we want 50% power when at the full extreme, // Full extreme is 900 ticks - // kP = (1.0 * 1023) / 900 = - motor.config_kP(0, 1.15, 0); // based upon Ken's initial calcs, above + // kP = (0.5 * 1023) / 900 = 0.568 + motor.config_kP(0, 0.5, 0); // based upon Ken's initial calcs, above // typical value of about 1/100 of kP for starting tuning motor.config_kI(0, 0.0, 0); // typical value of about 10x to 100x of kP for starting tuning - motor.config_kD(0, 0.0, 0); + motor.config_kD(0, 150.0, 0); // motor.config_kD(0, 0.575, 0); // practically always set kF to 0 for position control @@ -69,13 +66,13 @@ void configMotor(MayhemTalonSRX motor) { motor.setNeutralMode(NeutralMode.Coast); motor.setInverted(false); - motor.setSensorPhase(false); + motor.setSensorPhase(true); motor.configNominalOutputVoltage(+0.0f, -0.0f); motor.configPeakOutputVoltage(+12.0, -12.0); motor.configClosedloopRamp(0.05); // limit neutral to full to 0.05 seconds - motor.configMotionCruiseVelocity(300); // estimate 300 ticks max - motor.configMotionAcceleration(600); // acceleration of 2x velocity allows cruise to be attained in 1/2 - // second + motor.configMotionCruiseVelocity(30); // estimate 30 ticks max + motor.configMotionAcceleration(60); // acceleration of 2x velocity allows cruise to be attained in 1/2 + // second motor.setFeedbackDevice(FeedbackDevice.QuadEncoder); } @@ -94,23 +91,28 @@ public void setRollers(double power) { } public void setExtender(Double b) { + m_targetPosition = b; extenderTalon.set(ControlMode.Position, b); mode = PivotMode.PID_MODE; + isMoving = true; + } + + public boolean isExtenderAtPosition() { + return !isMoving; } @Override public void periodic() { // This method will be called once per scheduler run - switch (mode) { - case PID_MODE: + if (mode == PivotMode.PID_MODE) { + // if the pivot is close enough, turn off the motor - if (Math.abs(extenderTalon.getSetpoint() - extenderTalon.get()) < PIVOT_CLOSE_ENOUGH) { + if (Math.abs(extenderTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) { + isMoving = false; setExtenderVBus(0); } - break; - default: - break; + } else { } updateSmartDashBoard(); @@ -118,6 +120,10 @@ public void periodic() { public void updateSmartDashBoard() { SmartDashboard.putNumber("Intake Position", extenderTalon.getPosition()); + SmartDashboard.putNumber("Intake Target", m_targetPosition); + + SmartDashboard.putBoolean("Intake Is Moving", isMoving); + SmartDashboard.putBoolean("Intake PID Mode", (mode == PivotMode.PID_MODE)); SmartDashboard.putNumber("Intake Rollers", rollerTalon.get()); } diff --git a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java index 027c70d..1b4b247 100644 --- a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java +++ b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -17,6 +17,8 @@ public class MayhemTalonSRX extends TalonSRX { public MayhemTalonSRX(int deviceNumber) { super(deviceNumber); + this.configFactoryDefault(); + this.configNominalOutputForward(0.0, 0); this.configNominalOutputReverse(0.0, 0); this.configPeakOutputForward(1.0, 0); @@ -86,7 +88,7 @@ public void set(int deviceID) { } public void setFeedbackDevice(FeedbackDevice feedback) { - this.configSelectedFeedbackSensor(feedback, 0, 1000); + this.configSelectedFeedbackSensor(feedback, 0, 0); } public void reverseSensor(boolean b) { From eb3af1805e1e783e65c8b8af0b34257434a4fe3f Mon Sep 17 00:00:00 2001 From: robertdeml Date: Fri, 7 Feb 2020 22:37:25 -0500 Subject: [PATCH 044/121] Added shooter vbus Added ability to control the shooter using VBus % as first test. --- .../commands/ShooterAdjustWheelVBus.java | 33 +++++++++++++++++++ .../commands/ShooterSetWheelVBus.java | 32 ++++++++++++++++++ .../robot2020/subsystems/Shooter.java | 33 +++++++++++-------- 3 files changed, 85 insertions(+), 13 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheelVBus.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheelVBus.java diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheelVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheelVBus.java new file mode 100644 index 0000000..72c19fd --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheelVBus.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterAdjustWheelVBus extends InstantCommand { + double m_adjust; + + /** + * Creates a new ShooterAdjustWheelVBus. + */ + public ShooterAdjustWheelVBus(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double currentSpeed = RobotContainer.shooter.getShooterWheelSpeedVBus(); + RobotContainer.shooter.setShooterWheelSpeedVBus(currentSpeed + m_adjust); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheelVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheelVBus.java new file mode 100644 index 0000000..5f6c287 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheelVBus.java @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterSetWheelVBus extends InstantCommand { + + double m_speed; + /** + * Creates a new ShooterSetWheelVBus. + */ + public ShooterSetWheelVBus(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + + m_speed = speed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooter.setShooterWheelSpeedVBus(m_speed); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index c0d9fd3..b62d942 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -8,14 +8,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.mayheminc.util.MayhemFakeTalonSRX; import org.mayheminc.util.MayhemTalonSRX; public class Shooter extends SubsystemBase { - private final MayhemFakeTalonSRX shooterWheelTalon = new MayhemFakeTalonSRX(Constants.Talon.SHOOTER_WHEEL); - private final MayhemFakeTalonSRX turretTalon = new MayhemFakeTalonSRX(Constants.Talon.SHOOTER_TURRET); - private final MayhemFakeTalonSRX hoodTalon = new MayhemFakeTalonSRX(Constants.Talon.SHOOTER_HOOD); - private final MayhemFakeTalonSRX feederTalon = new MayhemFakeTalonSRX(Constants.Talon.SHOOTER_FEEDER); + private final MayhemTalonSRX shooterWheelTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL); + private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); + private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); + private final MayhemTalonSRX feederTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_FEEDER); /** * Creates a new Shooter. @@ -41,7 +40,7 @@ private void configureHoodTalon() { } private void configureWheelTalon() { - shooterWheelTalon.config_kP(0, 1.0, 0); + shooterWheelTalon.config_kP(0, 60.0, 0); shooterWheelTalon.config_kI(0, 0.0, 0); shooterWheelTalon.config_kD(0, 0.0, 0); shooterWheelTalon.config_kF(0, 0.0, 0); @@ -65,10 +64,10 @@ public void periodic() { } private void UpdateDashboard() { - SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelTalon.get()); - SmartDashboard.putNumber("Shooter turet pos", turretTalon.get()); - SmartDashboard.putNumber("Shooter hood pos", hoodTalon.get()); - SmartDashboard.putNumber("Shooter feeder speed", feederTalon.get()); + SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelTalon.getSpeed()); + SmartDashboard.putNumber("Shooter turet pos", turretTalon.getPosition()); + SmartDashboard.putNumber("Shooter hood pos", hoodTalon.getPosition()); + SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getPosition()); } public void zeroTurretPosition(int pos) { @@ -97,7 +96,7 @@ public void setTurretPositionRel(double pos) { * @return */ public double getTurretPosition() { - return turretTalon.get(); + return turretTalon.getPosition(); } public void setHoodPosition(double pos) { @@ -105,7 +104,7 @@ public void setHoodPosition(double pos) { } public double getHoodPosition() { - return hoodTalon.get(); + return hoodTalon.getPosition(); } public void setFeederSpeed(double pos) { @@ -114,9 +113,17 @@ public void setFeederSpeed(double pos) { public void setShooterWheelSpeed(double pos) { shooterWheelTalon.set(ControlMode.Velocity, pos); + } + public void setShooterWheelSpeedVBus(double pos) { + shooterWheelTalon.set(ControlMode.PercentOutput, pos); } public double getShooterWheelSpeed() { - return shooterWheelTalon.get(); + return shooterWheelTalon.getSpeed(); + } + + public double getShooterWheelSpeedVBus() { + return shooterWheelTalon.getOutputVoltage(); } + } \ No newline at end of file From 3a8f48f33620f3800f807c368e1dbed887478964 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Fri, 7 Feb 2020 22:38:01 -0500 Subject: [PATCH 045/121] Climber Pistons Added climber pistons and command to control. --- .../org/mayheminc/robot2020/Constants.java | 7 + .../mayheminc/robot2020/RobotContainer.java | 243 +++++++++--------- .../robot2020/commands/ClimberSetPistons.java | 31 +++ .../robot2020/subsystems/Climber.java | 25 +- 4 files changed, 179 insertions(+), 127 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ClimberSetPistons.java diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index b349aa5..2fefb9b 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -44,6 +44,13 @@ public final class Talon { public static final int CONTROL_PANEL_ROLLER = 17; // WON"T FIT!!! } + public final class Solenoid { + public static final int SPARE_0 = 0; + public static final int SPARE_1 = 1; + public static final int SPARE_2 = 2; + public static final int CLIMBER_PISTONS = 3; + } + public final class PDP { public static final int DRIVE_RIGHT_A = 1; public static final int DRIVE_RIGHT_B = 2; diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 81d6244..94892ff 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -26,122 +26,129 @@ * commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - - public static final Climber climber = new Climber(); - public static final Magazine magazine = new Magazine(); - public static final Shooter shooter = new Shooter(); - public static final Drive drive = new Drive(); - public static final Intake intake = new Intake(); - public static final Autonomous autonomous = new Autonomous(); - public static final Targeting targeting = new Targeting(); - - private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); - public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); - - public static final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); - private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); - - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() { - // Configure the button bindings - configureButtonBindings(); - configureAutonomousPrograms(); - configureDefaultCommands(); - } - - private void configureDefaultCommands() { - drive.setDefaultCommand(new DriveDefault()); - intake.setDefaultCommand(new IntakeExtenderVBus()); - } - - private void configureAutonomousPrograms() { - LinkedList autonomousPrograms = new LinkedList(); - - // autonomousPrograms.push(new StayStill()); - autonomousPrograms.push(new DriveStraight()); - // autonomousPrograms.push(new TestTurret()); - - autonomous.setAutonomousPrograms(autonomousPrograms); - - } - - /** - * Use this method to define your button->command mappings. Buttons can be - * created by instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then - * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() { - configureDriverStickButtons(); - configureDriverPadButtons(); - configureOperatorStickButtons(); - configureOperatorPadButtons(); - - } - - private void configureDriverStickButtons() { - - // DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); - // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); - - // // adjust auto parameters - DRIVER_STICK.DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(autonomous, 1)); - DRIVER_STICK.DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(autonomous, -1)); - DRIVER_STICK.DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(autonomous, -1)); - DRIVER_STICK.DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(autonomous, 1)); - - // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner - // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); - - // // zero elements that require zeroing - // DRIVER_STICK_BUTTON_EIGHT.whenPressed(new DriveZeroGyro()); - // DRIVER_STICK_BUTTON_NINE.whenPressed(new Wait(0)); - - } - - private void configureDriverPadButtons() { - } - - private void configureOperatorStickButtons() { - } - - private void configureOperatorPadButtons() { - OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new IntakeZero()); - // new ShooterAdjustWheel(-100)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO - .whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - // new ShooterSetWheel(0)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE - .whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - // new ShooterAdjustWheel(+100)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR - .whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); - // new ShooterSetWheel(1000)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(0.3)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); - - OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN - .whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2)); - - // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whenPressed(new - // IntakeSetPosition(0.0)); - // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whenPressed(new - // IntakeSetPosition(90.0)); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return new RunAutonomous(); - } + // The robot's subsystems and commands are defined here... + + public static final Climber climber = new Climber(); + public static final Magazine magazine = new Magazine(); + public static final Shooter shooter = new Shooter(); + public static final Drive drive = new Drive(); + public static final Intake intake = new Intake(); + public static final Autonomous autonomous = new Autonomous(); + public static final Targeting targeting = new Targeting(); + + private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); + public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); + + public static final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); + private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + configureAutonomousPrograms(); + configureDefaultCommands(); + } + + private void configureDefaultCommands() { + drive.setDefaultCommand(new DriveDefault()); + intake.setDefaultCommand(new IntakeExtenderVBus()); + } + + private void configureAutonomousPrograms() { + LinkedList autonomousPrograms = new LinkedList(); + + // autonomousPrograms.push(new StayStill()); + autonomousPrograms.push(new DriveStraight()); + // autonomousPrograms.push(new TestTurret()); + + autonomous.setAutonomousPrograms(autonomousPrograms); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + configureDriverStickButtons(); + configureDriverPadButtons(); + configureOperatorStickButtons(); + configureOperatorPadButtons(); + + } + + private void configureDriverStickButtons() { + + // DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); + // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); + + // // adjust auto parameters + DRIVER_STICK.DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(autonomous, 1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(autonomous, -1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(autonomous, -1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(autonomous, 1)); + + // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner + // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); + + // // zero elements that require zeroing + // DRIVER_STICK_BUTTON_EIGHT.whenPressed(new DriveZeroGyro()); + // DRIVER_STICK_BUTTON_NINE.whenPressed(new Wait(0)); + + } + + private void configureDriverPadButtons() { + + // Debug shooter + DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheelVBus(0.1)); + DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheelVBus(-0.1)); + DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterAdjustWheelVBus(0.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(0.5)); + + // debug climber pistons + DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ClimberSetPistons(true)); + DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ClimberSetPistons(false)); + + } + + private void configureOperatorStickButtons() { + } + + private void configureOperatorPadButtons() { + OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new IntakeZero()); + // new ShooterAdjustWheel(-100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + // new ShooterSetWheel(0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + // new ShooterAdjustWheel(+100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); + // new ShooterSetWheel(1000)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(0.3)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); + + OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2)); + + // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whenPressed(new + // IntakeSetPosition(0.0)); + // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whenPressed(new + // IntakeSetPosition(90.0)); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return new RunAutonomous(); + } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ClimberSetPistons.java b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetPistons.java new file mode 100644 index 0000000..e8e07c1 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetPistons.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ClimberSetPistons extends InstantCommand { + boolean m_piston; + + public ClimberSetPistons(boolean b) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.climber); + m_piston = b; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.climber.setPistons(m_piston); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index dbbc53d..c52c1dd 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -11,17 +11,19 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import org.mayheminc.robot2020.Constants; -import org.mayheminc.util.MayhemFakeTalonSRX; import org.mayheminc.util.MayhemTalonSRX; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { - private final MayhemFakeTalonSRX winchLeft = new MayhemFakeTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT); - private final MayhemFakeTalonSRX winchRight = new MayhemFakeTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT); - private final MayhemFakeTalonSRX walkerLeft = new MayhemFakeTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); - private final MayhemFakeTalonSRX walkerRight = new MayhemFakeTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); + private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT); + private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT); + private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); + private final MayhemTalonSRX walkerRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); + + private final Solenoid pistons = new Solenoid(Constants.Solenoid.CLIMBER_PISTONS); /** * Creates a new Climber. @@ -48,10 +50,11 @@ public Climber() { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Climber Winch Left", winchLeft.get()); - SmartDashboard.putNumber("Climber Winch Right", winchRight.get()); - SmartDashboard.putNumber("Climber Walker Left", walkerLeft.get()); - SmartDashboard.putNumber("Climber Walker Right", walkerRight.get()); + SmartDashboard.putNumber("Climber Winch Left", winchLeft.getPosition()); + SmartDashboard.putNumber("Climber Winch Right", winchRight.getPosition()); + SmartDashboard.putNumber("Climber Walker Left", walkerLeft.getPosition()); + SmartDashboard.putNumber("Climber Walker Right", walkerRight.getPosition()); + SmartDashboard.putBoolean("Climber Pistons", pistons.get()); } public void setWinchLeftSpeed(double power) { @@ -70,4 +73,8 @@ public void setWalkerRightSpeed(double power) { walkerRight.set(ControlMode.Velocity, power); } + public void setPistons(boolean b) { + pistons.set(b); + } + } \ No newline at end of file From 5025663acadf1efb016ecd029da8a7282a7defa9 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Fri, 7 Feb 2020 22:38:55 -0500 Subject: [PATCH 046/121] Tweaks to Mayhem talon removed bad methods of the Mayhem Talon and made changes to drive and intake to not use the bad methods. --- .../mayheminc/robot2020/subsystems/Drive.java | 2 +- .../robot2020/subsystems/Intake.java | 2 +- .../org/mayheminc/util/MayhemTalonSRX.java | 20 +++++++++---------- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 84a2c8b..716b502 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -161,7 +161,7 @@ private void configureCanTalon(MayhemTalonSRX talon) { talon.setFeedbackDevice(FeedbackDevice.QuadEncoder); - talon.reverseSensor(false); + // talon.reverseSensor(false); talon.configNominalOutputVoltage(+0.0f, -0.0f); talon.configPeakOutputVoltage(+12.0, -12.0); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index bb2fef4..dfbbeab 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -124,7 +124,7 @@ public void updateSmartDashBoard() { SmartDashboard.putBoolean("Intake Is Moving", isMoving); SmartDashboard.putBoolean("Intake PID Mode", (mode == PivotMode.PID_MODE)); - SmartDashboard.putNumber("Intake Rollers", rollerTalon.get()); + SmartDashboard.putNumber("Intake Rollers", rollerTalon.getOutputVoltage()); } public void setExtenderVBus(double VBus) { diff --git a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java index 1b4b247..6a6c355 100644 --- a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java +++ b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -91,9 +91,9 @@ public void setFeedbackDevice(FeedbackDevice feedback) { this.configSelectedFeedbackSensor(feedback, 0, 0); } - public void reverseSensor(boolean b) { + // public void reverseSensor(boolean b) { - } + // } public void configNominalOutputVoltage(float f, float g) { this.configNominalOutputForward(f / 12.0, 1000); @@ -114,9 +114,9 @@ public void setPID(double wheelP, double wheelI, double wheelD, double wheelF, i this.config_kF(pidProfile, wheelF, 1000); } - public double getSetpoint() { - return 0; - } + // public double getSetpoint() { + // return 0; + // } public double getError() { return this.getClosedLoopError(0); @@ -141,9 +141,9 @@ public void setVoltageRampRate(double d) { this.configClosedloopRamp(0, 0); } - public void enableControl() { + // public void enableControl() { - } + // } public void setPosition(int zeroPositionCount) { this.setSelectedSensorPosition(zeroPositionCount, 0, 1000); @@ -161,7 +161,7 @@ public void setEncPosition(int i) { setPosition(i); } - public double get() { - return this.getOutputCurrent(); - } + // public double get() { + // return this.getOutputCurrent(); + // } } From b929c8409bb9e288d936127e4df11b038df066bd Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 8 Feb 2020 16:17:29 -0500 Subject: [PATCH 047/121] PID Tuner, Gravity Assist, Init Robot Container. Call RobotContainer.init at the start of disabled to reinit the subsystems. Renamed Extender to pivot. Changed the intake to use Motion Magic to compensate for gravity. Shooter has the PID Tuner. --- src/main/java/frc/robot/Robot.java | 39 ++++++---- .../org/mayheminc/robot2020/Constants.java | 2 +- .../mayheminc/robot2020/RobotContainer.java | 37 +++++++--- .../commands/ClimberSetWinchesPower.java | 45 ++++++++++++ .../commands/IntakeExtenderVBus.java | 2 +- .../robot2020/commands/IntakeSetPosition.java | 4 +- .../robot2020/subsystems/Climber.java | 12 ++-- .../robot2020/subsystems/Intake.java | 58 +++++++++++---- .../robot2020/subsystems/Magazine.java | 9 ++- .../robot2020/subsystems/Shooter.java | 71 +++++++++++++++++-- .../java/org/mayheminc/util/PidTuner.java | 19 +++-- .../org/mayheminc/util/PidTunerObject.java | 7 ++ 12 files changed, 242 insertions(+), 63 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 59bb8c2..b95843e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,9 +14,10 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; /** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the TimedRobot + * documentation. If you change the name of this class or the package after + * creating this project, you must also update the build.gradle file in the * project. */ public class Robot extends TimedRobot { @@ -25,28 +26,34 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; /** - * This function is run when the robot is first started up and should be used for any - * initialization code. + * This function is run when the robot is first started up and should be used + * for any initialization code. */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); } /** - * This function is called every robot packet, no matter the mode. Use this for items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * This function is called every robot packet, no matter the mode. Use this for + * items like diagnostics that you want ran during disabled, autonomous, + * teleoperated and test. * - *

This runs after the mode specific periodic functions, but before - * LiveWindow and SmartDashboard integrated updating. + *

+ * This runs after the mode specific periodic functions, but before LiveWindow + * and SmartDashboard integrated updating. */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled + // commands, running already-scheduled commands, removing finished or + // interrupted commands, + // and running subsystem periodic() methods. This must be called from the + // robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); } @@ -56,6 +63,7 @@ public void robotPeriodic() { */ @Override public void disabledInit() { + RobotContainer.init(); } @Override @@ -63,7 +71,8 @@ public void disabledPeriodic() { } /** - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + * This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. */ @Override public void autonomousInit() { @@ -91,6 +100,8 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + + RobotContainer.shooter.init(); } /** diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 2fefb9b..9e9183e 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -31,7 +31,7 @@ public final class Talon { public static final int SHOOTER_HOOD = 8; public static final int INTAKE_ROLLERS = 9; - public static final int INTAKE_EXTENDER = 10; + public static final int INTAKE_PIVOT = 10; public static final int MAGAZINE_TURNTABLE = 11; public static final int MAGAZINE_CHIMNEY = 12; diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 94892ff..2403b4c 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Button; import java.util.LinkedList; @@ -35,10 +36,11 @@ public class RobotContainer { public static final Intake intake = new Intake(); public static final Autonomous autonomous = new Autonomous(); public static final Targeting targeting = new Targeting(); + public static PidTuner pidtuner; - private static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); + // Operator Inputs + public static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); - public static final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); @@ -50,6 +52,15 @@ public RobotContainer() { configureButtonBindings(); configureAutonomousPrograms(); configureDefaultCommands(); + + pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_BUTTON_SIX, + RobotContainer.DRIVER_STICK.DRIVER_STICK_BUTTON_SEVEN, + RobotContainer.DRIVER_STICK.DRIVER_STICK_BUTTON_ELEVEN, + RobotContainer.DRIVER_STICK.DRIVER_STICK_BUTTON_TEN, shooter); + } + + public static void init() { + shooter.init(); } private void configureDefaultCommands() { @@ -104,10 +115,18 @@ private void configureDriverStickButtons() { private void configureDriverPadButtons() { - // Debug shooter - DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheelVBus(0.1)); - DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheelVBus(-0.1)); - DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterAdjustWheelVBus(0.0)); + // Debug shooter vbus + // DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new + // ShooterAdjustWheelVBus(0.1)); + // DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new + // ShooterAdjustWheelVBus(-0.1)); + // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheelVBus(0.0)); + + // Debug shooter pid velocity + DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(25.0)); + DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-25.0)); + DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheel(0.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(0.5)); // debug climber pistons @@ -137,10 +156,8 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2)); - // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whenPressed(new - // IntakeSetPosition(0.0)); - // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whenPressed(new - // IntakeSetPosition(90.0)); + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whenPressed(new ClimberSetWinchesPower(0.5)); + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whenPressed(new ClimberSetWinchesPower(-0.5)); } /** diff --git a/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java new file mode 100644 index 0000000..d8536b5 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ClimberSetWinchesPower extends CommandBase { + double power; + + /** + * Creates a new ClimberSetWinchesPower. + */ + public ClimberSetWinchesPower(double d) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.climber); + power = d; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + RobotContainer.climber.setWinchLeftSpeed(power); + RobotContainer.climber.setWinchRightSpeed(power); + } + + @Override + public void end(boolean interrupted) { + + RobotContainer.climber.setWinchLeftSpeed(0.0); + RobotContainer.climber.setWinchRightSpeed(0.0); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java index 523c757..a471f5a 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java @@ -26,7 +26,7 @@ public void execute() { double power = RobotContainer.OPERATOR_PAD.getLeftYAxis(); double sign = (power >= 0) ? -1.0 : 1.0;// invert the sign power = sign * power * power; - RobotContainer.intake.setExtenderVBus(power); + RobotContainer.intake.setPivotVBus(power); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java index 2c1911a..1e2e318 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java @@ -27,12 +27,12 @@ public IntakeSetPosition(Double position) { // Called when the command is initially scheduled. @Override public void initialize() { - RobotContainer.intake.setExtender(m_position); + RobotContainer.intake.setPivot(m_position); } // Returns true when the command should end. @Override public boolean isFinished() { - return RobotContainer.intake.isExtenderAtPosition(); + return RobotContainer.intake.isPivotAtPosition(); } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index c52c1dd..5845998 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -29,19 +29,19 @@ public class Climber extends SubsystemBase { * Creates a new Climber. */ public Climber() { - winchLeft.setNeutralMode(NeutralMode.Brake); + winchLeft.setNeutralMode(NeutralMode.Coast); winchLeft.configNominalOutputVoltage(+0.0f, -0.0f); winchLeft.configPeakOutputVoltage(+12.0, -12.0); - winchRight.setNeutralMode(NeutralMode.Brake); + winchRight.setNeutralMode(NeutralMode.Coast); winchRight.configNominalOutputVoltage(+0.0f, -0.0f); winchRight.configPeakOutputVoltage(+12.0, -12.0); - walkerRight.setNeutralMode(NeutralMode.Brake); + walkerRight.setNeutralMode(NeutralMode.Coast); walkerRight.configNominalOutputVoltage(+0.0f, -0.0f); walkerRight.configPeakOutputVoltage(+12.0, -12.0); - walkerLeft.setNeutralMode(NeutralMode.Brake); + walkerLeft.setNeutralMode(NeutralMode.Coast); walkerLeft.configNominalOutputVoltage(+0.0f, -0.0f); walkerLeft.configPeakOutputVoltage(+12.0, -12.0); @@ -58,11 +58,11 @@ public void periodic() { } public void setWinchLeftSpeed(double power) { - winchLeft.set(ControlMode.Velocity, power); + winchLeft.set(ControlMode.PercentOutput, power); } public void setWinchRightSpeed(double power) { - winchRight.set(ControlMode.Velocity, power); + winchRight.set(ControlMode.PercentOutput, power); } public void setWalkerLeftSpeed(double power) { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index dfbbeab..2957820 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -8,6 +8,7 @@ package org.mayheminc.robot2020.subsystems; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -21,11 +22,13 @@ public class Intake extends SubsystemBase { private final int PIVOT_CLOSE_ENOUGH = 20; private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_ROLLERS); - private final MayhemTalonSRX extenderTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_EXTENDER); + private final MayhemTalonSRX pivotTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_PIVOT); private final int PIVOT_ZERO_POSITION = 900; public final double PIVOT_UP = 900.0; public final double PIVOT_DOWN = 0.0; + private static final double HORIZONTAL_HOLD_OUTPUT = 0.08; + enum PivotMode { MANUAL_MODE, PID_MODE, }; @@ -33,6 +36,7 @@ enum PivotMode { PivotMode mode = PivotMode.MANUAL_MODE; boolean isMoving; double m_targetPosition; + double m_feedForward; /** * Creates a new Intake. @@ -42,7 +46,7 @@ public Intake() { rollerTalon.configNominalOutputVoltage(+0.0f, -0.0f); rollerTalon.configPeakOutputVoltage(+12.0, -12.0); - configMotor(extenderTalon); + configMotor(pivotTalon); } void configMotor(MayhemTalonSRX motor) { @@ -77,7 +81,7 @@ void configMotor(MayhemTalonSRX motor) { } public void zero() { - extenderTalon.setEncPosition(PIVOT_ZERO_POSITION); + pivotTalon.setEncPosition(PIVOT_ZERO_POSITION); } /** @@ -90,14 +94,15 @@ public void setRollers(double power) { } - public void setExtender(Double b) { + public void setPivot(Double b) { m_targetPosition = b; - extenderTalon.set(ControlMode.Position, b); + // pivotTalon.set(ControlMode.Position, b); + mode = PivotMode.PID_MODE; isMoving = true; } - public boolean isExtenderAtPosition() { + public boolean isPivotAtPosition() { return !isMoving; } @@ -105,21 +110,48 @@ public boolean isExtenderAtPosition() { public void periodic() { // This method will be called once per scheduler run + updateSmartDashBoard(); + updateSensorPosition(); + updatePivotPower(); + } + + private void updatePivotPower() { + if (mode == PivotMode.PID_MODE) { // if the pivot is close enough, turn off the motor - if (Math.abs(extenderTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) { + if (Math.abs(pivotTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) { isMoving = false; - setExtenderVBus(0); + setPivotVBus(0); + } else { + pivotTalon.set(ControlMode.MotionMagic, m_targetPosition, DemandType.ArbitraryFeedForward, m_feedForward); } - } else { } + } - updateSmartDashBoard(); + void updateSensorPosition() { + int m_currentPosition = pivotTalon.getPosition(); + double m_angleInDegrees = positionToDegrees(m_currentPosition); + double m_angleInRadians = Math.toRadians(m_angleInDegrees); + + // get a range of -1 to 1 to multiply by feedforward. + // when in horizontal forward position, value should be 1 + // when in vertical up or down position, value should be 0 + // when in horizontal backward position, value should be -1 + double m_gravityCompensation = Math.cos(m_angleInRadians); + + // HORIZONTAL_HOLD_OUTPUT is the minimum power required to hold the arm up when + // horizontal + // this is a range of -1.0 to 1.0 (%vbus), determined empirically + m_feedForward = m_gravityCompensation * HORIZONTAL_HOLD_OUTPUT; + } + + private double positionToDegrees(int pos) { + return pos / 10; // 900 encoder ticks per 90 degrees } public void updateSmartDashBoard() { - SmartDashboard.putNumber("Intake Position", extenderTalon.getPosition()); + SmartDashboard.putNumber("Intake Position", pivotTalon.getPosition()); SmartDashboard.putNumber("Intake Target", m_targetPosition); SmartDashboard.putBoolean("Intake Is Moving", isMoving); @@ -127,8 +159,8 @@ public void updateSmartDashBoard() { SmartDashboard.putNumber("Intake Rollers", rollerTalon.getOutputVoltage()); } - public void setExtenderVBus(double VBus) { - extenderTalon.set(ControlMode.PercentOutput, VBus); + public void setPivotVBus(double VBus) { + pivotTalon.set(ControlMode.PercentOutput, VBus); mode = PivotMode.MANUAL_MODE; } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java index 50efa08..abdceeb 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java @@ -14,12 +14,11 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import org.mayheminc.robot2020.Constants; -import org.mayheminc.util.MayhemFakeTalonSRX; import org.mayheminc.util.MayhemTalonSRX; public class Magazine extends SubsystemBase { - private final MayhemFakeTalonSRX turntableTalon = new MayhemFakeTalonSRX(Constants.Talon.MAGAZINE_TURNTABLE); - private final MayhemFakeTalonSRX chimneyTalen = new MayhemFakeTalonSRX(Constants.Talon.MAGAZINE_CHIMNEY); + private final MayhemTalonSRX turntableTalon = new MayhemTalonSRX(Constants.Talon.MAGAZINE_TURNTABLE); + private final MayhemTalonSRX chimneyTalen = new MayhemTalonSRX(Constants.Talon.MAGAZINE_CHIMNEY); /** * Creates a new Magazine. @@ -29,7 +28,7 @@ public Magazine() { ConfigureTalon(chimneyTalen); } - private void ConfigureTalon(MayhemFakeTalonSRX talon) { + private void ConfigureTalon(MayhemTalonSRX talon) { talon.setNeutralMode(NeutralMode.Coast); talon.configNominalOutputVoltage(+0.0f, -0.0f); talon.configPeakOutputVoltage(+12.0, -12.0); @@ -38,7 +37,7 @@ private void ConfigureTalon(MayhemFakeTalonSRX talon) { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Magazine Turntable", turntableTalon.get()); + SmartDashboard.putNumber("Magazine Turntable", turntableTalon.getSpeed()); } public void setTurntableSpeed(double speed) { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index b62d942..bd70d31 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -1,6 +1,7 @@ package org.mayheminc.robot2020.subsystems; import org.mayheminc.robot2020.Constants; +import org.mayheminc.robot2020.RobotContainer; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -9,21 +10,31 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.PidTuner; +import org.mayheminc.util.PidTunerObject; -public class Shooter extends SubsystemBase { +public class Shooter extends SubsystemBase implements PidTunerObject { private final MayhemTalonSRX shooterWheelTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL); private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); private final MayhemTalonSRX feederTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_FEEDER); + double m_targetSpeed; + /** * Creates a new Shooter. */ public Shooter() { + + } + + public void init() { configureTurretTalon(); configureWheelTalon(); configureHoodTalon(); configureFeederTalon(); + + setShooterWheelSpeedVBus(0.0); } private void configureFeederTalon() { @@ -46,6 +57,7 @@ private void configureWheelTalon() { shooterWheelTalon.config_kF(0, 0.0, 0); shooterWheelTalon.changeControlMode(ControlMode.Velocity); shooterWheelTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + } void configureTurretTalon() { @@ -60,11 +72,14 @@ void configureTurretTalon() { @Override public void periodic() { // This method will be called once per scheduler run + UpdateDashboard(); } private void UpdateDashboard() { SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelTalon.getSpeed()); + SmartDashboard.putNumber("Shooter Wheel target", m_targetSpeed); + SmartDashboard.putNumber("Shooter turet pos", turretTalon.getPosition()); SmartDashboard.putNumber("Shooter hood pos", hoodTalon.getPosition()); SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getPosition()); @@ -112,18 +127,64 @@ public void setFeederSpeed(double pos) { } public void setShooterWheelSpeed(double pos) { + m_targetSpeed = pos; shooterWheelTalon.set(ControlMode.Velocity, pos); - } + } + public void setShooterWheelSpeedVBus(double pos) { shooterWheelTalon.set(ControlMode.PercentOutput, pos); } public double getShooterWheelSpeed() { return shooterWheelTalon.getSpeed(); - } - - public double getShooterWheelSpeedVBus() { + } + + public double getShooterWheelSpeedVBus() { return shooterWheelTalon.getOutputVoltage(); } + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return shooterWheelTalon.getP(); + } + + @Override + public double getI() { + return shooterWheelTalon.getI(); + } + + @Override + public double getD() { + return shooterWheelTalon.getD(); + } + + @Override + public double getF() { + return shooterWheelTalon.getF(); + + } + + @Override + public void setP(double d) { + shooterWheelTalon.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + shooterWheelTalon.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + shooterWheelTalon.config_kD(0, d, 0); + + } + + @Override + public void setF(double d) { + shooterWheelTalon.config_kF(0, d, 0); + } + } \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/PidTuner.java b/src/main/java/org/mayheminc/util/PidTuner.java index bd1975b..52b952e 100644 --- a/src/main/java/org/mayheminc/util/PidTuner.java +++ b/src/main/java/org/mayheminc/util/PidTuner.java @@ -8,15 +8,15 @@ * This is a class to help tune a PID. Another class that extends * IPidTunerObject is passed in. This interface defines methods that are used to * set the P-I-D values. This class gets joystick buttons that define the - * different options. - Button 1: cycle through P-I-D values - Button 2: + * different options. - Button 1: cycle through P-I-D-F values - Button 2: * increase the value. - Button 3: decrease the value. - Button 4: cycle through * the amount the value changes: 10, 1, .1, .01, .001, .0001 This class needs to - * be called for updateSmartDashboard(). + * be called from updateSmartDashboard(). * * @author User * */ -public class PidTuner extends InstantCommand { +public class PidTuner extends InstantCommand implements Subsystem { Button m_PidCycle; Button m_Inc; Button m_Dec; @@ -34,6 +34,8 @@ enum PidCycle { // remember the PID Object so we can get/set the PID values. public PidTuner(final Button b1, final Button b2, final Button b3, final Button b4, final PidTunerObject obj) { + CommandScheduler.getInstance().registerSubsystem(this); + b1.whenPressed(this); b2.whenPressed(this); b3.whenPressed(this); @@ -67,7 +69,7 @@ public void initialize() { } } - // set the P, I, or D that we are changing + // set the P, I, or D or F that we are changing public void RunCycle() { // System.out.println("PID Tuner: RunCycle"); m_cycle++; @@ -112,7 +114,7 @@ public void RunInc() { return 0.0; } - // based on the cycle, set the P, I, or D. + // based on the cycle, set the P, I, or D or F. void setValue(final double d) { switch (m_cycle) { case 0: @@ -151,7 +153,12 @@ public void RunValue() { } } - public void updateSmartDashboard() { + @Override + public void periodic() { + updateSmartDashboard(); + } + + private void updateSmartDashboard() { SmartDashboard.putNumber("PID Tuner P", m_pidObj.getP()); SmartDashboard.putNumber("PID Tuner I", m_pidObj.getI()); SmartDashboard.putNumber("PID Tuner D", m_pidObj.getD()); diff --git a/src/main/java/org/mayheminc/util/PidTunerObject.java b/src/main/java/org/mayheminc/util/PidTunerObject.java index d9a59d0..1c77ded 100644 --- a/src/main/java/org/mayheminc/util/PidTunerObject.java +++ b/src/main/java/org/mayheminc/util/PidTunerObject.java @@ -3,11 +3,18 @@ public interface PidTunerObject { double getP(); + double getI(); + double getD(); + double getF(); + void setP(double d); + void setI(double d); + void setD(double d); + void setF(double d); } From 81004e9624e488aae758cd70e10dad716e14cd98 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 8 Feb 2020 21:43:10 -0500 Subject: [PATCH 048/121] shooter spins, intake pivots. --- src/main/java/frc/robot/Robot.java | 2 +- .../org/mayheminc/robot2020/Constants.java | 4 +- .../mayheminc/robot2020/RobotContainer.java | 23 ++++--- .../robot2020/commands/ClimberZero.java | 28 ++++++++ .../commands/ShooterAdjustWheel.java | 2 +- .../robot2020/commands/ShooterZero.java | 28 ++++++++ .../commands/SystemZeroIncludingGyro.java | 31 +++++++++ .../robot2020/subsystems/Climber.java | 20 ++++-- .../mayheminc/robot2020/subsystems/Drive.java | 17 ++--- .../robot2020/subsystems/Intake.java | 6 +- .../robot2020/subsystems/Shooter.java | 68 +++++++++++++++---- 11 files changed, 182 insertions(+), 47 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ClimberZero.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterZero.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b95843e..df67473 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -101,7 +101,7 @@ public void teleopInit() { m_autonomousCommand.cancel(); } - RobotContainer.shooter.init(); + // RobotContainer.compressor.start(); } /** diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 9e9183e..49939fe 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -36,8 +36,8 @@ public final class Talon { public static final int MAGAZINE_TURNTABLE = 11; public static final int MAGAZINE_CHIMNEY = 12; - public static final int CLIMBER_WINCH_LEFT = 13; // high current - public static final int CLIMBER_WINCH_RIGHT = 14; // high current + public static final int CLIMBER_WINCH_LEFT = 14; // high current + public static final int CLIMBER_WINCH_RIGHT = 13; // high current public static final int CLIMBER_WALKER_LEFT = 15; public static final int CLIMBER_WALKER_RIGHT = 16; diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 2403b4c..844457f 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -9,6 +9,7 @@ import org.mayheminc.util.*; +import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.Button; @@ -36,6 +37,7 @@ public class RobotContainer { public static final Intake intake = new Intake(); public static final Autonomous autonomous = new Autonomous(); public static final Targeting targeting = new Targeting(); + // public static final Compressor compressor = new Compressor(); public static PidTuner pidtuner; // Operator Inputs @@ -95,7 +97,7 @@ private void configureButtonBindings() { private void configureDriverStickButtons() { - // DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); + DRIVER_STICK.DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); // // adjust auto parameters @@ -123,15 +125,15 @@ private void configureDriverPadButtons() { // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheelVBus(0.0)); // Debug shooter pid velocity - DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(25.0)); - DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-25.0)); + DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(100.0)); + DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-100.0)); DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheel(0.0)); DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(0.5)); // debug climber pistons - DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ClimberSetPistons(true)); - DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ClimberSetPistons(false)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ClimberSetPistons(true)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ClimberSetPistons(false)); } @@ -139,11 +141,12 @@ private void configureOperatorStickButtons() { } private void configureOperatorPadButtons() { - OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new IntakeZero()); + // OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new IntakeZero()); // new ShooterAdjustWheel(-100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); // new ShooterSetWheel(0)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + // OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new + // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); // new ShooterAdjustWheel(+100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // new ShooterSetWheel(1000)); @@ -156,8 +159,10 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2)); - OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whenPressed(new ClimberSetWinchesPower(0.5)); - OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whenPressed(new ClimberSetWinchesPower(-0.5)); + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(0.2)); + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new ClimberSetPistons(true)); + OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whenPressed(new ClimberSetPistons(false)); } /** diff --git a/src/main/java/org/mayheminc/robot2020/commands/ClimberZero.java b/src/main/java/org/mayheminc/robot2020/commands/ClimberZero.java new file mode 100644 index 0000000..42138d1 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ClimberZero.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ClimberZero extends InstantCommand { + public ClimberZero() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.climber.zero(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java index c96adfb..e0e1d20 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java @@ -28,7 +28,7 @@ public ShooterAdjustWheel(double adjust) { // Called when the command is initially scheduled. @Override public void initialize() { - RobotContainer.shooter.setShooterWheelSpeed(RobotContainer.shooter.getShooterWheelSpeed() + m_adjust); + RobotContainer.shooter.setShooterWheelSpeed(RobotContainer.shooter.getShooterWheelTargetSpeed() + m_adjust); } // Returns true when the command should end. diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterZero.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterZero.java new file mode 100644 index 0000000..88e4fe4 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterZero.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShooterZero extends InstantCommand { + public ShooterZero() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooter.zero(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java b/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java new file mode 100644 index 0000000..d75dcfe --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class SystemZeroIncludingGyro extends SequentialCommandGroup { + /** + * Creates a new SystemZeroIncludingGyro. + */ + public SystemZeroIncludingGyro() { + // Use addRequirements() here to declare subsystem dependencies. + + addCommands(new IntakeZero()); + addCommands(new ClimberZero()); + addCommands(new ShooterZero()); + addCommands(new DriveZeroGyro()); + } + + @Override + public boolean runsWhenDisabled() { + // TODO Auto-generated method stub + return true; + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 5845998..4a43943 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -23,7 +23,8 @@ public class Climber extends SubsystemBase { private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); private final MayhemTalonSRX walkerRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); - private final Solenoid pistons = new Solenoid(Constants.Solenoid.CLIMBER_PISTONS); + // private final Solenoid pistons = new + // Solenoid(Constants.Solenoid.CLIMBER_PISTONS); /** * Creates a new Climber. @@ -47,14 +48,19 @@ public Climber() { } + public void zero() { + winchLeft.setSelectedSensorPosition(0); + winchRight.setSelectedSensorPosition(0); + } + @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Climber Winch Left", winchLeft.getPosition()); - SmartDashboard.putNumber("Climber Winch Right", winchRight.getPosition()); - SmartDashboard.putNumber("Climber Walker Left", walkerLeft.getPosition()); - SmartDashboard.putNumber("Climber Walker Right", walkerRight.getPosition()); - SmartDashboard.putBoolean("Climber Pistons", pistons.get()); + // SmartDashboard.putNumber("Climber Winch Left", winchLeft.getPosition()); + // SmartDashboard.putNumber("Climber Winch Right", winchRight.getPosition()); + // SmartDashboard.putNumber("Climber Walker Left", walkerLeft.getPosition()); + // SmartDashboard.putNumber("Climber Walker Right", walkerRight.getPosition()); + // SmartDashboard.putBoolean("Climber Pistons", pistons.get()); } public void setWinchLeftSpeed(double power) { @@ -74,7 +80,7 @@ public void setWalkerRightSpeed(double power) { } public void setPistons(boolean b) { - pistons.set(b); + // pistons.set(b); } } \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 716b502..ab11262 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -159,7 +159,7 @@ private void configureCanTalon(MayhemTalonSRX talon) { double wheelD = 0.0; double wheelF = 1.0; - talon.setFeedbackDevice(FeedbackDevice.QuadEncoder); + talon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); // talon.reverseSensor(false); talon.configNominalOutputVoltage(+0.0f, -0.0f); @@ -587,20 +587,17 @@ public void updateSmartDashboard() { SmartDashboard.putNumber("Right Rear Encoder Counts", rightRearTalon.getSelectedSensorPosition(0)); // Note: getSpeed() returns ticks per 0.1 seconds - // SmartDashboard.putNumber("Left Encoder Speed", - // leftFrontTalon.getSelectedSensorVelocity(0)); - // SmartDashboard.putNumber("Right Encoder Speed", - // rightFrontTalon.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Left Encoder Speed", leftFrontTalon.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Right Encoder Speed", rightFrontTalon.getSelectedSensorVelocity(0)); // To convert ticks per 0.1 seconds into feet per second // a - multiply be 10 (tenths of second per second) // b - divide by 12 (1 foot per 12 inches) // c - multiply by distance (in inches) per pulse - // SmartDashboard.putNumber("Left Speed (fps)", - // leftFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE); - // SmartDashboard.putNumber("Right Speed (fps)", - // rightFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * - // DISTANCE_PER_PULSE); + SmartDashboard.putNumber("Left Speed (fps)", + leftFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE); + SmartDashboard.putNumber("Right Speed (fps)", + rightFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE); SmartDashboard.putNumber("Left Talon Output Voltage", leftFrontTalon.getOutputVoltage()); SmartDashboard.putNumber("Right Talon Output Voltage", rightFrontTalon.getOutputVoltage()); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 2957820..2351c9d 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -27,7 +27,7 @@ public class Intake extends SubsystemBase { public final double PIVOT_UP = 900.0; public final double PIVOT_DOWN = 0.0; - private static final double HORIZONTAL_HOLD_OUTPUT = 0.08; + private static final double HORIZONTAL_HOLD_OUTPUT = 0.00; enum PivotMode { MANUAL_MODE, PID_MODE, @@ -54,7 +54,7 @@ void configMotor(MayhemTalonSRX motor) { // If we want 50% power when at the full extreme, // Full extreme is 900 ticks // kP = (0.5 * 1023) / 900 = 0.568 - motor.config_kP(0, 0.5, 0); // based upon Ken's initial calcs, above + motor.config_kP(0, 0.5, 0); // based upon Robert's initial calcs, above // typical value of about 1/100 of kP for starting tuning motor.config_kI(0, 0.0, 0); @@ -124,7 +124,7 @@ private void updatePivotPower() { isMoving = false; setPivotVBus(0); } else { - pivotTalon.set(ControlMode.MotionMagic, m_targetPosition, DemandType.ArbitraryFeedForward, m_feedForward); + pivotTalon.set(ControlMode.Position, m_targetPosition, DemandType.ArbitraryFeedForward, m_feedForward); } } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index bd70d31..c1feb73 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -19,13 +19,27 @@ public class Shooter extends SubsystemBase implements PidTunerObject { private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); private final MayhemTalonSRX feederTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_FEEDER); - double m_targetSpeed; + private final double MAX_SPEED_RPM = 5760.0; + private final double TALON_TICKS_PER_REV = 2048.0; + + double m_targetSpeedRPM; + + double convertRpmToTicksPer100ms(double rpm) { + return rpm / 60 * 2048 / 10; + } + + double convertTicksPer100msToRPM(double ticks) { + return ticks * 10 / 2048 * 60; + } /** * Creates a new Shooter. */ public Shooter() { - + configureTurretTalon(); + configureWheelTalon(); + configureHoodTalon(); + configureFeederTalon(); } public void init() { @@ -39,6 +53,9 @@ public void init() { private void configureFeederTalon() { feederTalon.changeControlMode(ControlMode.PercentOutput); + + feederTalon.configNominalOutputVoltage(+0.0f, -0.0f); + feederTalon.configPeakOutputVoltage(+6.0, -6.0); } private void configureHoodTalon() { @@ -46,18 +63,24 @@ private void configureHoodTalon() { hoodTalon.config_kI(0, 0.0, 0); hoodTalon.config_kD(0, 0.0, 0); hoodTalon.config_kF(0, 0.0, 0); + hoodTalon.changeControlMode(ControlMode.Position); hoodTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + hoodTalon.configNominalOutputVoltage(+0.0f, -0.0f); + hoodTalon.configPeakOutputVoltage(+6.0, -6.0); } private void configureWheelTalon() { - shooterWheelTalon.config_kP(0, 60.0, 0); + shooterWheelTalon.config_kP(0, 0.0, 0); shooterWheelTalon.config_kI(0, 0.0, 0); shooterWheelTalon.config_kD(0, 0.0, 0); - shooterWheelTalon.config_kF(0, 0.0, 0); - shooterWheelTalon.changeControlMode(ControlMode.Velocity); - shooterWheelTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + shooterWheelTalon.config_kF(0, 1023.0 / convertRpmToTicksPer100ms(5760), 0); + shooterWheelTalon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); + + shooterWheelTalon.configNominalOutputVoltage(+0.0f, -0.0f); + shooterWheelTalon.configPeakOutputVoltage(+12.0, 0.0); } void configureTurretTalon() { @@ -67,6 +90,9 @@ void configureTurretTalon() { turretTalon.config_kF(0, 0.0, 0); turretTalon.changeControlMode(ControlMode.Position); turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + turretTalon.configNominalOutputVoltage(+0.0f, -0.0f); + turretTalon.configPeakOutputVoltage(+6.0, -6.0); } @Override @@ -76,17 +102,26 @@ public void periodic() { UpdateDashboard(); } + int debugShooter; + private void UpdateDashboard() { - SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelTalon.getSpeed()); - SmartDashboard.putNumber("Shooter Wheel target", m_targetSpeed); + SmartDashboard.putNumber("Shooter Wheel pos", shooterWheelTalon.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelTalon.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Shooter Wheel RPM", + convertTicksPer100msToRPM(shooterWheelTalon.getSelectedSensorVelocity(0))); + + SmartDashboard.putNumber("Shooter Wheel target RPM", m_targetSpeedRPM); SmartDashboard.putNumber("Shooter turet pos", turretTalon.getPosition()); SmartDashboard.putNumber("Shooter hood pos", hoodTalon.getPosition()); SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getPosition()); + + SmartDashboard.putNumber("Shooter Debug", debugShooter++); } - public void zeroTurretPosition(int pos) { - turretTalon.setPosition(pos); + public void zero() { + turretTalon.setPosition(0); + hoodTalon.setPosition(0); } /** @@ -126,9 +161,10 @@ public void setFeederSpeed(double pos) { feederTalon.set(ControlMode.PercentOutput, pos); } - public void setShooterWheelSpeed(double pos) { - m_targetSpeed = pos; - shooterWheelTalon.set(ControlMode.Velocity, pos); + public void setShooterWheelSpeed(double rpm) { + double ticks = convertRpmToTicksPer100ms(rpm); + m_targetSpeedRPM = rpm; + shooterWheelTalon.set(ControlMode.Velocity, ticks); } public void setShooterWheelSpeedVBus(double pos) { @@ -136,7 +172,11 @@ public void setShooterWheelSpeedVBus(double pos) { } public double getShooterWheelSpeed() { - return shooterWheelTalon.getSpeed(); + return convertTicksPer100msToRPM(shooterWheelTalon.getSelectedSensorVelocity(0)); + } + + public double getShooterWheelTargetSpeed() { + return m_targetSpeedRPM; } public double getShooterWheelSpeedVBus() { From ce3c46cebca731b4fd51d4a5d0b824ed09eb426b Mon Sep 17 00:00:00 2001 From: robertdeml Date: Tue, 11 Feb 2020 19:19:11 -0500 Subject: [PATCH 049/121] added magazine default --- .../mayheminc/robot2020/RobotContainer.java | 3 +- .../robot2020/commands/MagazineDefault.java | 46 +++++++ .../org/mayheminc/util/MayhemOperatorPad.java | 113 ++++++++++-------- 3 files changed, 112 insertions(+), 50 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 844457f..6079e43 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -67,7 +67,8 @@ public static void init() { private void configureDefaultCommands() { drive.setDefaultCommand(new DriveDefault()); - intake.setDefaultCommand(new IntakeExtenderVBus()); + // intake.setDefaultCommand(new IntakeExtenderVBus()); + magazine.setDefaultCommand(new MagazineDefault()); } private void configureAutonomousPrograms() { diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java new file mode 100644 index 0000000..4fb4bec --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class MagazineDefault extends CommandBase { + /** + * Creates a new MagazineDefault. + */ + public MagazineDefault() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.magazine); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + RobotContainer.magazine.setChimneySpeed(RobotContainer.OPERATOR_PAD.getLeftYAxis()); + RobotContainer.magazine.setTurntableSpeed(RobotContainer.OPERATOR_PAD.getLeftYAxis()); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java index 60835a0..f10250a 100644 --- a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java +++ b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java @@ -6,61 +6,76 @@ public class MayhemOperatorPad { - public final class OPERATOR_PAD_AXIS { - public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; - public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; - public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; - public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; - } - - public final Joystick OPERATOR_PAD = new Joystick(Joysticks.OPERATOR_GAMEPAD); - public final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); - public final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); - public final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); - public final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); - public final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); - public final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); - public final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); - public final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); - public final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); - public final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); - public final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); - public final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); - public final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); - - public final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); - public final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); - public final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); - public final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); - - // Operator Control Buttons - public final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - public final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - public final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, - OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); - public final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, - OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); - + public final class OPERATOR_PAD_AXIS { public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; + } + + public final Joystick OPERATOR_PAD = new Joystick(Joysticks.OPERATOR_GAMEPAD); + public final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); + public final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); + public final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); + public final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); + public final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); + public final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); + public final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); + public final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); + public final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); + public final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); + public final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); + public final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); + public final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); + + public final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); + public final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); + public final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); + public final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); + + // Operator Control Buttons + public final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + public final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + public final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + public final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + + public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; + public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; + public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; + public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; + + private static final double Y_AXIS_DEAD_ZONE_PERCENT = 0.15; + private static final double X_AXIS_DEAD_ZONE_PERCENT = 0.15; + + public double getLeftYAxis() { + double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_LEFT_Y_AXIS); + return ApplyDeadZone(value, Y_AXIS_DEAD_ZONE_PERCENT); + } + + public double getLeftXAxis() { + double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_LEFT_X_AXIS); + return ApplyDeadZone(value, X_AXIS_DEAD_ZONE_PERCENT); + } + + public double getRightYAxis() { + double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_RIGHT_Y_AXIS); + return ApplyDeadZone(value, Y_AXIS_DEAD_ZONE_PERCENT); + } - private static final double Y_AXIS_DEAD_ZONE_PERCENT = 0.15; + public double getRightXAxis() { + double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_LEFT_X_AXIS); + return ApplyDeadZone(value, X_AXIS_DEAD_ZONE_PERCENT); + } - public double getLeftYAxis() { - // SteeringX is the "X" axis of the right stick on the Driver Gamepad. - double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_LEFT_Y_AXIS); - if (Math.abs(value) < Y_AXIS_DEAD_ZONE_PERCENT) { - value = 0.0; - } + private double ApplyDeadZone(double value, double deadZone) { - // if the slow button is pressed, cut the steering value in half. - // if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { - // value = value / 2.0; - // } - return value; + if (Math.abs(value) < deadZone) { + return 0; } + return value; + } } From c708917b9bab9c119b39895286c005976d10e7ca Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Tue, 11 Feb 2020 20:08:08 -0500 Subject: [PATCH 050/121] seperated the chimney from the magazine subsystem --- .../mayheminc/robot2020/RobotContainer.java | 1 + .../robot2020/commands/MagazineDefault.java | 1 - .../commands/MagazineSetChimney.java | 2 +- .../robot2020/subsystems/Chimney.java | 49 +++++++++++++++++++ .../robot2020/subsystems/Magazine.java | 17 ++++--- 5 files changed, 62 insertions(+), 8 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 6079e43..d099de6 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -39,6 +39,7 @@ public class RobotContainer { public static final Targeting targeting = new Targeting(); // public static final Compressor compressor = new Compressor(); public static PidTuner pidtuner; + public static final Chimney chimney = new Chimney(); // Operator Inputs public static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java index 4fb4bec..d6b1ece 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java @@ -28,7 +28,6 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - RobotContainer.magazine.setChimneySpeed(RobotContainer.OPERATOR_PAD.getLeftYAxis()); RobotContainer.magazine.setTurntableSpeed(RobotContainer.OPERATOR_PAD.getLeftYAxis()); } diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java index cfd2834..1a7d918 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java @@ -26,7 +26,7 @@ public MagazineSetChimney(double d) { // Called when the command is initially scheduled. @Override public void initialize() { - RobotContainer.magazine.setChimneySpeed(m_speed); + RobotContainer.chimney.setChimneySpeed(m_speed); } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java new file mode 100644 index 0000000..4ead261 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.util.MayhemTalonSRX; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Chimney extends SubsystemBase { + private final MayhemTalonSRX chimneyTalon = new MayhemTalonSRX(Constants.Talon.MAGAZINE_CHIMNEY); + + /** + * Creates a new Chimney. + */ + public Chimney() { + chimneyTalon.setNeutralMode(NeutralMode.Coast); + chimneyTalon.configNominalOutputVoltage(+0.0f, -0.0f); + chimneyTalon.configPeakOutputVoltage(+12.0, -12.0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + updateSmartDashboard(); + monitorTurntableMovement(); + } + + void updateSmartDashboard() { + SmartDashboard.putNumber("Magazine Turntable", chimneyTalon.getSpeed()); + } + + void monitorTurntableMovement() { + + } + + public void setChimneySpeed(double speed) { + chimneyTalon.set(ControlMode.PercentOutput, speed); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java index abdceeb..0ae7129 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import org.mayheminc.robot2020.Constants; @@ -18,14 +19,13 @@ public class Magazine extends SubsystemBase { private final MayhemTalonSRX turntableTalon = new MayhemTalonSRX(Constants.Talon.MAGAZINE_TURNTABLE); - private final MayhemTalonSRX chimneyTalen = new MayhemTalonSRX(Constants.Talon.MAGAZINE_CHIMNEY); /** * Creates a new Magazine. */ public Magazine() { ConfigureTalon(turntableTalon); - ConfigureTalon(chimneyTalen); + turntableTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); } private void ConfigureTalon(MayhemTalonSRX talon) { @@ -37,15 +37,20 @@ private void ConfigureTalon(MayhemTalonSRX talon) { @Override public void periodic() { // This method will be called once per scheduler run + updateSmartDashboard(); + monitorTurntableMovement(); + } + + void updateSmartDashboard() { SmartDashboard.putNumber("Magazine Turntable", turntableTalon.getSpeed()); } - public void setTurntableSpeed(double speed) { - turntableTalon.set(ControlMode.PercentOutput, speed); + void monitorTurntableMovement() { + } - public void setChimneySpeed(double speed) { - chimneyTalen.set(ControlMode.PercentOutput, speed); + public void setTurntableSpeed(double speed) { + turntableTalon.set(ControlMode.PercentOutput, speed); } } From 7730d6b0ab05920080ac3bdcc58f3cc9c46cd402 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Tue, 11 Feb 2020 20:59:22 -0500 Subject: [PATCH 051/121] moved chimney from magazine into its own subsystem --- .../{MagazineSetChimney.java => ChimneySetChimney.java} | 8 ++++---- .../mayheminc/robot2020/commands/ShooterReadyAimFire.java | 4 ++-- .../java/org/mayheminc/robot2020/subsystems/Chimney.java | 3 +-- 3 files changed, 7 insertions(+), 8 deletions(-) rename src/main/java/org/mayheminc/robot2020/commands/{MagazineSetChimney.java => ChimneySetChimney.java} (89%) diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java b/src/main/java/org/mayheminc/robot2020/commands/ChimneySetChimney.java similarity index 89% rename from src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java rename to src/main/java/org/mayheminc/robot2020/commands/ChimneySetChimney.java index 1a7d918..1fe4861 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ChimneySetChimney.java @@ -11,15 +11,15 @@ import edu.wpi.first.wpilibj2.command.CommandBase; -public class MagazineSetChimney extends CommandBase { +public class ChimneySetChimney extends CommandBase { double m_speed; /** * Creates a new MagazineSetChimney. */ - public MagazineSetChimney(double d) { + public ChimneySetChimney(double d) { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.magazine); + addRequirements(RobotContainer.chimney); m_speed = d; } @@ -39,4 +39,4 @@ public void end(boolean interrupted) { public boolean isFinished() { return true; } -} +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index ef0ff1b..4a9e0d2 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -24,9 +24,9 @@ public ShooterReadyAimFire() { super(); addCommands(new TargetingIsOnTarget()); addCommands(new MagazineSetTurntable(0.3)); - addCommands(new MagazineSetChimney(0.3)); + addCommands(new ChimneySetChimney(0.3)); addCommands(new ParallelRaceGroup(new ShooterFireWhenReady(), new Wait(5.0))); addCommands(new MagazineSetTurntable(0.0)); - addCommands(new MagazineSetChimney(0.0)); + addCommands(new ChimneySetChimney(0.0)); } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java index 4ead261..62c9945 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -36,11 +36,10 @@ public void periodic() { } void updateSmartDashboard() { - SmartDashboard.putNumber("Magazine Turntable", chimneyTalon.getSpeed()); + SmartDashboard.putNumber("Chimney Turntable", chimneyTalon.getSpeed()); } void monitorTurntableMovement() { - } public void setChimneySpeed(double speed) { From 2bd86cde9acf4d99165dd3612444d75b3c30adc3 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Tue, 11 Feb 2020 21:10:15 -0500 Subject: [PATCH 052/121] minor adjustments to driver --- src/main/java/org/mayheminc/robot2020/RobotContainer.java | 6 +++--- src/main/java/org/mayheminc/robot2020/subsystems/Drive.java | 2 +- .../java/org/mayheminc/robot2020/subsystems/Shooter.java | 2 ++ 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 844457f..3d28741 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -150,7 +150,7 @@ private void configureOperatorPadButtons() { // new ShooterAdjustWheel(+100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // new ShooterSetWheel(1000)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetTurntable(0.3)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetChimney(1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); @@ -159,8 +159,8 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2)); - OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(0.2)); - OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(0.7)); + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-0.7)); OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new ClimberSetPistons(true)); OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whenPressed(new ClimberSetPistons(false)); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index ab11262..227ce2c 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -419,7 +419,7 @@ public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickT double rotation = 0; double adjustedSteeringX = rawSteeringX * throttle; final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast. - final double STD_TURN_GAIN = 1.0; // 2019: 1.5. 2020: 1.5 was too fast// the driver wants the non-quick turn + final double STD_TURN_GAIN = 0.9; // 2019: 1.5. 2020: 1.5 was too fast// the driver wants the non-quick turn // turning a little more responsive. int throttleSign; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index c1feb73..0f86eb0 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -111,6 +111,8 @@ private void UpdateDashboard() { convertTicksPer100msToRPM(shooterWheelTalon.getSelectedSensorVelocity(0))); SmartDashboard.putNumber("Shooter Wheel target RPM", m_targetSpeedRPM); + SmartDashboard.putNumber("Shooter Wheel Error", + m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelTalon.getSelectedSensorVelocity(0))); SmartDashboard.putNumber("Shooter turet pos", turretTalon.getPosition()); SmartDashboard.putNumber("Shooter hood pos", hoodTalon.getPosition()); From 0140934f7b18e5abe29887fb0cf7e6d24a6ce8d3 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Tue, 11 Feb 2020 21:13:25 -0500 Subject: [PATCH 053/121] changed chimney talon to true --- src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java index 4ead261..a0d5024 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -26,6 +26,7 @@ public Chimney() { chimneyTalon.setNeutralMode(NeutralMode.Coast); chimneyTalon.configNominalOutputVoltage(+0.0f, -0.0f); chimneyTalon.configPeakOutputVoltage(+12.0, -12.0); + chimneyTalon.setInverted(true); } @Override From f457090446812ecf91733e3954ecf3c1e3eeedb5 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Wed, 12 Feb 2020 18:21:28 -0500 Subject: [PATCH 054/121] changed MagazineSetChimney to ChimneySetChimney --- src/main/java/org/mayheminc/robot2020/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 5d2690e..4cb120d 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -152,7 +152,7 @@ private void configureOperatorPadButtons() { // new ShooterAdjustWheel(+100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // new ShooterSetWheel(1000)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetChimney(1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySetChimney(1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); From 3a2f325261e99a1bf148746579749301bb73bc74 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Thu, 13 Feb 2020 17:54:23 -0500 Subject: [PATCH 055/121] Revert "changed MagazineSetChimney to ChimneySetChimney" This reverts commit f457090446812ecf91733e3954ecf3c1e3eeedb5. --- src/main/java/org/mayheminc/robot2020/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 4cb120d..5d2690e 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -152,7 +152,7 @@ private void configureOperatorPadButtons() { // new ShooterAdjustWheel(+100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // new ShooterSetWheel(1000)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySetChimney(1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetChimney(1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); From 74c594eb8976f6ea61e323e035272af80541403b Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Thu, 13 Feb 2020 17:54:51 -0500 Subject: [PATCH 056/121] Create Compressor.java --- .../robot2020/subsystems/Compressor.java | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Compressor.java diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Compressor.java b/src/main/java/org/mayheminc/robot2020/subsystems/Compressor.java new file mode 100644 index 0000000..8d0af8a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Compressor.java @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class compressor extends SubsystemBase { + /** + * Creates a new compressor. + */ + public compressor() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From d9d0273514680ab850d673491085bfcfd72915c2 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Thu, 13 Feb 2020 19:34:03 -0500 Subject: [PATCH 057/121] added AirCompresorDefault --- .../mayheminc/robot2020/RobotContainer.java | 4 +- .../commands/AirCompressorDefault.java | 44 +++++++++++++++++++ .../commands/MagazineSetTurntable.java | 1 + .../{Compressor.java => AirCompressor.java} | 14 +++++- 4 files changed, 59 insertions(+), 4 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java rename src/main/java/org/mayheminc/robot2020/subsystems/{Compressor.java => AirCompressor.java} (71%) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 5d2690e..a415351 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -37,7 +37,7 @@ public class RobotContainer { public static final Intake intake = new Intake(); public static final Autonomous autonomous = new Autonomous(); public static final Targeting targeting = new Targeting(); - // public static final Compressor compressor = new Compressor(); + public static final AirCompressor compressor = new AirCompressor(); public static PidTuner pidtuner; public static final Chimney chimney = new Chimney(); @@ -152,7 +152,7 @@ private void configureOperatorPadButtons() { // new ShooterAdjustWheel(+100)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // new ShooterSetWheel(1000)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new MagazineSetChimney(1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySetChimney(1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java new file mode 100644 index 0000000..e19dd66 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class AirCompressorDefault extends CommandBase { + /** + * Creates a new AirCompressorDefault. + */ + public AirCompressorDefault() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // robot.is + RobotContainer.compressor.setCompresor(true); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java index a0de4ea..3bb1bb6 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java @@ -9,6 +9,7 @@ import org.mayheminc.robot2020.RobotContainer; +import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj2.command.CommandBase; public class MagazineSetTurntable extends CommandBase { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Compressor.java b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java similarity index 71% rename from src/main/java/org/mayheminc/robot2020/subsystems/Compressor.java rename to src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java index 8d0af8a..95d88b6 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Compressor.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java @@ -7,14 +7,24 @@ package org.mayheminc.robot2020.subsystems; +import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class compressor extends SubsystemBase { +public class AirCompressor extends SubsystemBase { + Compressor compressor = new Compressor(); + /** * Creates a new compressor. */ - public compressor() { + public AirCompressor() { + } + public void setCompresor(boolean b) { + if (b) { + compressor.start(); + } else { + compressor.stop(); + } } @Override From 7265fa433457dad1aff030b97ef69908b9f80cdc Mon Sep 17 00:00:00 2001 From: robertdeml Date: Thu, 13 Feb 2020 19:38:57 -0500 Subject: [PATCH 058/121] changed magazine to chimney --- .../org/mayheminc/robot2020/RobotContainer.java | 9 +++++---- .../robot2020/commands/ClimberSetWinchesPower.java | 2 +- .../robot2020/commands/MagazineSetChimney.java | 5 +++-- .../mayheminc/robot2020/subsystems/Chimney.java | 3 ++- .../mayheminc/robot2020/subsystems/Climber.java | 11 ++++++----- .../mayheminc/robot2020/subsystems/Magazine.java | 3 ++- .../mayheminc/robot2020/subsystems/Shooter.java | 14 ++++++++++---- .../java/org/mayheminc/util/MayhemDriverStick.java | 5 +++++ 8 files changed, 34 insertions(+), 18 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 5d2690e..130f59a 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -56,10 +56,10 @@ public RobotContainer() { configureAutonomousPrograms(); configureDefaultCommands(); - pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_BUTTON_SIX, - RobotContainer.DRIVER_STICK.DRIVER_STICK_BUTTON_SEVEN, - RobotContainer.DRIVER_STICK.DRIVER_STICK_BUTTON_ELEVEN, - RobotContainer.DRIVER_STICK.DRIVER_STICK_BUTTON_TEN, shooter); + pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX, + RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN, + RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN, + RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, shooter); } public static void init() { @@ -130,6 +130,7 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(100.0)); DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-100.0)); DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheel(0.0)); + DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(4600)); DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(0.5)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java index d8536b5..d753739 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java @@ -31,7 +31,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - RobotContainer.climber.setWinchLeftSpeed(power); + RobotContainer.climber.setWinchLeftSpeed(-power); RobotContainer.climber.setWinchRightSpeed(power); } diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java index 1a7d918..a57c148 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetChimney.java @@ -19,7 +19,7 @@ public class MagazineSetChimney extends CommandBase { */ public MagazineSetChimney(double d) { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.magazine); + addRequirements(RobotContainer.chimney); m_speed = d; } @@ -32,11 +32,12 @@ public void initialize() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + RobotContainer.chimney.setChimneySpeed(0.0); } // Returns true when the command should end. @Override public boolean isFinished() { - return true; + return false; } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java index a0d5024..2ce7d07 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -37,7 +37,8 @@ public void periodic() { } void updateSmartDashboard() { - SmartDashboard.putNumber("Magazine Turntable", chimneyTalon.getSpeed()); + SmartDashboard.putNumber("Chimney Speed", chimneyTalon.getSpeed()); + SmartDashboard.putNumber("Chimney Current", chimneyTalon.getStatorCurrent()); } void monitorTurntableMovement() { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 4a43943..d2a1b21 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -30,19 +30,20 @@ public class Climber extends SubsystemBase { * Creates a new Climber. */ public Climber() { - winchLeft.setNeutralMode(NeutralMode.Coast); + winchLeft.setNeutralMode(NeutralMode.Brake); winchLeft.configNominalOutputVoltage(+0.0f, -0.0f); winchLeft.configPeakOutputVoltage(+12.0, -12.0); winchRight.setNeutralMode(NeutralMode.Coast); + winchRight.setNeutralMode(NeutralMode.Brake); winchRight.configNominalOutputVoltage(+0.0f, -0.0f); winchRight.configPeakOutputVoltage(+12.0, -12.0); - walkerRight.setNeutralMode(NeutralMode.Coast); + walkerRight.setNeutralMode(NeutralMode.Brake); walkerRight.configNominalOutputVoltage(+0.0f, -0.0f); walkerRight.configPeakOutputVoltage(+12.0, -12.0); - walkerLeft.setNeutralMode(NeutralMode.Coast); + walkerLeft.setNeutralMode(NeutralMode.Brake); walkerLeft.configNominalOutputVoltage(+0.0f, -0.0f); walkerLeft.configPeakOutputVoltage(+12.0, -12.0); @@ -56,8 +57,8 @@ public void zero() { @Override public void periodic() { // This method will be called once per scheduler run - // SmartDashboard.putNumber("Climber Winch Left", winchLeft.getPosition()); - // SmartDashboard.putNumber("Climber Winch Right", winchRight.getPosition()); + SmartDashboard.putNumber("Climber Winch Left", winchLeft.getPosition()); + SmartDashboard.putNumber("Climber Winch Right", winchRight.getPosition()); // SmartDashboard.putNumber("Climber Walker Left", walkerLeft.getPosition()); // SmartDashboard.putNumber("Climber Walker Right", walkerRight.getPosition()); // SmartDashboard.putBoolean("Climber Pistons", pistons.get()); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java index 0ae7129..cc74ef8 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java @@ -42,7 +42,8 @@ public void periodic() { } void updateSmartDashboard() { - SmartDashboard.putNumber("Magazine Turntable", turntableTalon.getSpeed()); + SmartDashboard.putNumber("Magazine Speed", turntableTalon.getSpeed()); + // SmartDashboard.putNumber("Magazine Speed", turntableTalon.getSpeed()); } void monitorTurntableMovement() { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 0f86eb0..fc85832 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -40,6 +40,11 @@ public Shooter() { configureWheelTalon(); configureHoodTalon(); configureFeederTalon(); + + shooterWheelTalon.config_kP(0, 3.0, 0); + shooterWheelTalon.config_kI(0, 0.0, 0); + shooterWheelTalon.config_kD(0, 0.0, 0); + shooterWheelTalon.config_kF(0, 0.048);// 1023.0 / convertRpmToTicksPer100ms(5760), 0); } public void init() { @@ -72,10 +77,6 @@ private void configureHoodTalon() { } private void configureWheelTalon() { - shooterWheelTalon.config_kP(0, 0.0, 0); - shooterWheelTalon.config_kI(0, 0.0, 0); - shooterWheelTalon.config_kD(0, 0.0, 0); - shooterWheelTalon.config_kF(0, 1023.0 / convertRpmToTicksPer100ms(5760), 0); shooterWheelTalon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); @@ -163,6 +164,11 @@ public void setFeederSpeed(double pos) { feederTalon.set(ControlMode.PercentOutput, pos); } + /** + * Set shooter to rpm speed. + * + * @param rpm + */ public void setShooterWheelSpeed(double rpm) { double ticks = convertRpmToTicksPer100ms(rpm); m_targetSpeedRPM = rpm; diff --git a/src/main/java/org/mayheminc/util/MayhemDriverStick.java b/src/main/java/org/mayheminc/util/MayhemDriverStick.java index 7230b61..43963a6 100644 --- a/src/main/java/org/mayheminc/util/MayhemDriverStick.java +++ b/src/main/java/org/mayheminc/util/MayhemDriverStick.java @@ -21,6 +21,11 @@ public class MayhemDriverStick { public final Button DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(Joystick, 10); public final Button DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(Joystick, 11); + public final Button DRIVER_STICK_ENA_BUTTON_SIX = new EnabledOnlyJoystickButton(Joystick, 6); + public final Button DRIVER_STICK_ENA_BUTTON_SEVEN = new EnabledOnlyJoystickButton(Joystick, 7); + public final Button DRIVER_STICK_ENA_BUTTON_TEN = new EnabledOnlyJoystickButton(Joystick, 10); + public final Button DRIVER_STICK_ENA_BUTTON_ELEVEN = new EnabledOnlyJoystickButton(Joystick, 11); + public final int DRIVER_STICK_X_AXIS = 0; public final int DRIVER_STICK_Y_AXIS = 1; From 99073784e938ec728c381158b87fa98dca69c6d0 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Thu, 13 Feb 2020 19:43:01 -0500 Subject: [PATCH 059/121] Update Climber.java --- src/main/java/org/mayheminc/robot2020/subsystems/Climber.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index d2a1b21..9e10acb 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -34,7 +34,6 @@ public Climber() { winchLeft.configNominalOutputVoltage(+0.0f, -0.0f); winchLeft.configPeakOutputVoltage(+12.0, -12.0); - winchRight.setNeutralMode(NeutralMode.Coast); winchRight.setNeutralMode(NeutralMode.Brake); winchRight.configNominalOutputVoltage(+0.0f, -0.0f); winchRight.configPeakOutputVoltage(+12.0, -12.0); From 2eac5f30614db3473ed58686608f2ddccb55f5e4 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Sat, 15 Feb 2020 00:42:40 -0500 Subject: [PATCH 060/121] pre week 0 --- .../org/mayheminc/robot2020/Constants.java | 2 + .../mayheminc/robot2020/RobotContainer.java | 57 +++- .../autonomousroutines/DriveStraight.java | 4 +- .../ShootAndDriveForward.java | 29 ++ .../commands/AirCompressorDefault.java | 1 + .../robot2020/commands/MagazineDefault.java | 2 +- .../commands/ShooterFireWhenReady.java | 7 +- .../robot2020/commands/ShooterSetHoodAbs.java | 33 +++ ...terSetHood.java => ShooterSetHoodRel.java} | 4 +- .../commands/ShooterSetHoodVBus.java | 48 ++++ .../commands/ShooterSetTurretVBus.java | 48 ++++ .../robot2020/commands/ShooterSetWheel.java | 8 +- .../commands/TargetingIsOnTarget.java | 20 +- .../robot2020/commands/TurretAimToTarget.java | 45 +++ .../robot2020/subsystems/AirCompressor.java | 1 + .../robot2020/subsystems/Climber.java | 5 +- .../mayheminc/robot2020/subsystems/Drive.java | 12 +- .../robot2020/subsystems/Intake.java | 50 +++- .../robot2020/subsystems/Shooter.java | 75 ++++- .../robot2020/subsystems/Targeting.java | 258 ++++++++++++++++-- .../org/mayheminc/util/MayhemTalonSRX.java | 10 +- 21 files changed, 638 insertions(+), 81 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodAbs.java rename src/main/java/org/mayheminc/robot2020/commands/{ShooterSetHood.java => ShooterSetHoodRel.java} (91%) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodVBus.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretVBus.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 49939fe..b29f2b4 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -49,6 +49,8 @@ public final class Solenoid { public static final int SPARE_1 = 1; public static final int SPARE_2 = 2; public static final int CLIMBER_PISTONS = 3; + + public static final int CAMERA_LIGHTS = 7; } public final class PDP { diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 133cec0..7e404f0 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.Button; @@ -41,6 +42,8 @@ public class RobotContainer { public static PidTuner pidtuner; public static final Chimney chimney = new Chimney(); + public static final Solenoid cameraLights = new Solenoid(Constants.Solenoid.CAMERA_LIGHTS); + // Operator Inputs public static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); @@ -59,7 +62,9 @@ public RobotContainer() { pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN, - RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, shooter); + RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, intake); + + cameraLights.set(true); } public static void init() { @@ -70,6 +75,7 @@ private void configureDefaultCommands() { drive.setDefaultCommand(new DriveDefault()); // intake.setDefaultCommand(new IntakeExtenderVBus()); magazine.setDefaultCommand(new MagazineDefault()); + compressor.setDefaultCommand(new AirCompressorDefault()); } private void configureAutonomousPrograms() { @@ -126,11 +132,25 @@ private void configureDriverPadButtons() { // ShooterAdjustWheelVBus(-0.1)); // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheelVBus(0.0)); + // Debug Turret + // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new + // ShooterSetTurretAbs(-5500));// about -30 degrees + // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new + // ShooterSetTurretAbs(+5500));// about +30 degrees + DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));// about -30 degrees + DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2));// about +30 degrees + + // Debug Hood + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetHoodRel(+1000)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodRel(-1000)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); + // Debug shooter pid velocity DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(100.0)); DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-100.0)); DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheel(0.0)); - DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(4600)); + DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(2700)); DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(0.5)); @@ -144,28 +164,37 @@ private void configureOperatorStickButtons() { } private void configureOperatorPadButtons() { - // OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new IntakeZero()); - // new ShooterAdjustWheel(-100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new MagazineSetTurntable(0.2)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - // new ShooterSetWheel(0)); - // OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new - // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - // new ShooterAdjustWheel(+100)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new MagazineSetTurntable(1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); + // new ShooterSetWheel(1000)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySetChimney(1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whileHeld(new TurretAimToTarget()); + + OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(true)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TEN.whenPressed(new ClimberSetPistons(false)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whenPressed(new ShooterSetHood(-0.2)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whenPressed(new ShooterSetHood(-0.2)); + // OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new + // IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); + // OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new + // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(0.7)); OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-0.7)); - OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new ClimberSetPistons(true)); - OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whenPressed(new ClimberSetPistons(false)); + + // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new + // MagazineSetTurntable()); + // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whenPressed(new + // ClimberSetPistons(false)); } /** diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java index a67454b..ad4b0ae 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java @@ -20,8 +20,8 @@ public DriveStraight() { addCommands(new DriveZeroGyro()); addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 10)); - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 20)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 0)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0)); // addCommands(new ParallelCommandGroup(new IntakeSetPosition(true), new // MagazineSetTurntable(0.0))); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java new file mode 100644 index 0000000..884f9be --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.ShooterSetWheel; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShootAndDriveForward extends SequentialCommandGroup { + /** + * Creates a new ShootAndDriveForward. + */ + public ShootAndDriveForward() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + addCommands(new ShooterSetWheel(5500)); + // addCommands(new Wait(2.0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java index e19dd66..88ee715 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java +++ b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java @@ -17,6 +17,7 @@ public class AirCompressorDefault extends CommandBase { */ public AirCompressorDefault() { // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.compressor); } // Called when the command is initially scheduled. diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java index d6b1ece..2310374 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java @@ -28,7 +28,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - RobotContainer.magazine.setTurntableSpeed(RobotContainer.OPERATOR_PAD.getLeftYAxis()); + RobotContainer.magazine.setTurntableSpeed(-RobotContainer.OPERATOR_PAD.getLeftYAxis()); } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java index 61eb2ac..bac3dbc 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java @@ -29,10 +29,11 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - boolean wheelsGood = Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget()) - - RobotContainer.shooter.getShooterWheelSpeed()) < 100; + // boolean wheelsGood = + // Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget()) + // - RobotContainer.shooter.getShooterWheelSpeed()) < 100; - RobotContainer.shooter.setFeederSpeed((wheelsGood) ? 0.5 : 0.0); + // RobotContainer.shooter.setFeederSpeed((wheelsGood) ? 0.5 : 0.0); } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodAbs.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodAbs.java new file mode 100644 index 0000000..fa642b5 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodAbs.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterSetHoodAbs extends InstantCommand { + double m_set; + + /** + * Creates a new ShooterSetHoodAbs. + */ + public ShooterSetHoodAbs(double set) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + + m_set = set; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooter.setHoodPosition(m_set); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHood.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodRel.java similarity index 91% rename from src/main/java/org/mayheminc/robot2020/commands/ShooterSetHood.java rename to src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodRel.java index 0c44528..7980987 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHood.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodRel.java @@ -11,13 +11,13 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ShooterSetHood extends InstantCommand { +public class ShooterSetHoodRel extends InstantCommand { double m_adjust; /** * Creates a new ShooterSetHood. */ - public ShooterSetHood(double adjust) { + public ShooterSetHoodRel(double adjust) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.shooter); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodVBus.java new file mode 100644 index 0000000..5c566f0 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodVBus.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterSetHoodVBus extends CommandBase { + double m_power; + + /** + * Creates a new ShooterSetHoodVBus. + */ + public ShooterSetHoodVBus(double power) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + m_power = power; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooter.setHoodVBus(m_power); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.shooter.setHoodVBus(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretVBus.java new file mode 100644 index 0000000..205df2c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretVBus.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterSetTurretVBus extends CommandBase { + double m_power; + + /** + * Creates a new ShooterSetTurretVBus. + */ + public ShooterSetTurretVBus(double power) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + m_power = power; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooter.setTurretVBus(m_power); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.shooter.setTurretVBus(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java index 53fd88a..4acf4c0 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java @@ -12,22 +12,22 @@ import edu.wpi.first.wpilibj2.command.CommandBase; public class ShooterSetWheel extends CommandBase { - double m_adjust; + double m_rpm; /** * Creates a new ShooterSetWheel. */ - public ShooterSetWheel(double adjust) { + public ShooterSetWheel(double rpm) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.shooter); - m_adjust = adjust; + m_rpm = rpm; } // Called when the command is initially scheduled. @Override public void initialize() { - RobotContainer.shooter.setShooterWheelSpeed(m_adjust); + RobotContainer.shooter.setShooterWheelSpeed(m_rpm); } // Returns true when the command should end. diff --git a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java index 3c88df0..cff4995 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java @@ -28,11 +28,12 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double bearingToTarget = RobotContainer.targeting.getBearingToTarget(); - double rangeToTarget = RobotContainer.targeting.getRangeToTarget(); + // double bearingToTarget = RobotContainer.targeting.getBearingToTarget(); + // double rangeToTarget = RobotContainer.targeting.getRangeToTarget(); - RobotContainer.shooter.setTurretPositionRel(RobotContainer.shooter.getTurretPosition() + bearingToTarget); - RobotContainer.shooter.setShooterWheelSpeed(Targeting.convertRangeToWheelSpeed(rangeToTarget)); + // RobotContainer.shooter.setTurretPositionRel(RobotContainer.shooter.getTurretPosition() + // + bearingToTarget); + // RobotContainer.shooter.setShooterWheelSpeed(Targeting.convertRangeToWheelSpeed(rangeToTarget)); } // Called once the command ends or is interrupted. @@ -43,9 +44,12 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - boolean bearingGood = Math.abs(RobotContainer.targeting.getBearingToTarget()) < 2; - boolean wheelsGood = Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget()) - - RobotContainer.shooter.getShooterWheelSpeed()) < 100; - return bearingGood && wheelsGood; + // boolean bearingGood = Math.abs(RobotContainer.targeting.getBearingToTarget()) + // < 2; + // boolean wheelsGood = + // Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget()) + // - RobotContainer.shooter.getShooterWheelSpeed()) < 100; + // return bearingGood && wheelsGood; + return true; } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java new file mode 100644 index 0000000..850f632 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretAimToTarget extends CommandBase { + /** + * Creates a new TurretAimToTarget. + */ + public TurretAimToTarget() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double pos = RobotContainer.targeting.getDesiredAzimuth(); + RobotContainer.shooter.setTurretPositionAbs(pos); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java index 95d88b6..6b915cd 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java @@ -20,6 +20,7 @@ public AirCompressor() { } public void setCompresor(boolean b) { + // b = false; if (b) { compressor.start(); } else { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 9e10acb..3076a9f 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -23,8 +23,7 @@ public class Climber extends SubsystemBase { private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); private final MayhemTalonSRX walkerRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); - // private final Solenoid pistons = new - // Solenoid(Constants.Solenoid.CLIMBER_PISTONS); + private final Solenoid pistons = new Solenoid(Constants.Solenoid.CLIMBER_PISTONS); /** * Creates a new Climber. @@ -80,7 +79,7 @@ public void setWalkerRightSpeed(double power) { } public void setPistons(boolean b) { - // pistons.set(b); + pistons.set(b); } } \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 227ce2c..953f2cb 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -441,10 +441,7 @@ public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickT // not using camera targeting right now // check for if steering input is essentially zero - if (false /* - * turn off heading correction for now (-0.01 < rawSteeringX) && (rawSteeringX < - * 0.01) - */) { + if ((-0.01 < rawSteeringX) && (rawSteeringX < 0.01)) { // no turn being commanded, drive straight or stay still m_iterationsSinceRotationCommanded++; if ((-0.01 < throttle) && (throttle < 0.01)) { @@ -578,6 +575,9 @@ public void periodic() { public void updateSmartDashboard() { displayGyroInfo(); + SmartDashboard.putBoolean("In Autonomous", DriverStation.getInstance().isAutonomous()); + SmartDashboard.putNumber("Battery Voltage", RobotController.getBatteryVoltage()); + // ***** KBS: Uncommenting below, as it takes a LONG time to get PDP values // updateSdbPdp(); @@ -599,8 +599,8 @@ public void updateSmartDashboard() { SmartDashboard.putNumber("Right Speed (fps)", rightFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE); - SmartDashboard.putNumber("Left Talon Output Voltage", leftFrontTalon.getOutputVoltage()); - SmartDashboard.putNumber("Right Talon Output Voltage", rightFrontTalon.getOutputVoltage()); + SmartDashboard.putNumber("Left Talon Output Voltage", leftFrontTalon.getMotorOutputVoltage()); + SmartDashboard.putNumber("Right Talon Output Voltage", rightFrontTalon.getMotorOutputVoltage()); SmartDashboard.putBoolean("Closed Loop Mode", m_closedLoopMode); SmartDashboard.putBoolean("Speed Racer Drive Mode", m_speedRacerDriveMode); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 2351c9d..6342cea 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -14,13 +14,14 @@ import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.PidTunerObject; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Intake extends SubsystemBase { +public class Intake extends SubsystemBase implements PidTunerObject { - private final int PIVOT_CLOSE_ENOUGH = 20; + private final int PIVOT_CLOSE_ENOUGH = 50; private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_ROLLERS); private final MayhemTalonSRX pivotTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_PIVOT); private final int PIVOT_ZERO_POSITION = 900; @@ -54,7 +55,7 @@ void configMotor(MayhemTalonSRX motor) { // If we want 50% power when at the full extreme, // Full extreme is 900 ticks // kP = (0.5 * 1023) / 900 = 0.568 - motor.config_kP(0, 0.5, 0); // based upon Robert's initial calcs, above + motor.config_kP(0, 1.2, 0); // based upon Robert's initial calcs, above // typical value of about 1/100 of kP for starting tuning motor.config_kI(0, 0.0, 0); @@ -153,14 +154,55 @@ private double positionToDegrees(int pos) { public void updateSmartDashBoard() { SmartDashboard.putNumber("Intake Position", pivotTalon.getPosition()); SmartDashboard.putNumber("Intake Target", m_targetPosition); + SmartDashboard.putNumber("Intake FeedForward", m_feedForward); SmartDashboard.putBoolean("Intake Is Moving", isMoving); SmartDashboard.putBoolean("Intake PID Mode", (mode == PivotMode.PID_MODE)); - SmartDashboard.putNumber("Intake Rollers", rollerTalon.getOutputVoltage()); + SmartDashboard.putNumber("Intake Rollers", rollerTalon.getMotorOutputVoltage()); } public void setPivotVBus(double VBus) { pivotTalon.set(ControlMode.PercentOutput, VBus); mode = PivotMode.MANUAL_MODE; } + + @Override + public double getP() { + return this.pivotTalon.getP(); + } + + @Override + public double getI() { + return this.pivotTalon.getI(); + } + + @Override + public double getD() { + return this.pivotTalon.getD(); + } + + @Override + public double getF() { + return this.pivotTalon.getF(); + } + + @Override + public void setP(double d) { + this.pivotTalon.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + this.pivotTalon.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + this.pivotTalon.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + this.pivotTalon.config_kF(0, d, 0); + } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index fc85832..db0f6b8 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -6,9 +6,11 @@ import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.mayheminc.util.History; import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.PidTuner; import org.mayheminc.util.PidTunerObject; @@ -21,8 +23,11 @@ public class Shooter extends SubsystemBase implements PidTunerObject { private final double MAX_SPEED_RPM = 5760.0; private final double TALON_TICKS_PER_REV = 2048.0; + private final double TURRET_MIN_POS = -5500.0; + private final double TURRET_MAX_POS = +5500.0; double m_targetSpeedRPM; + History headingHistory = new History(); double convertRpmToTicksPer100ms(double rpm) { return rpm / 60 * 2048 / 10; @@ -74,6 +79,13 @@ private void configureHoodTalon() { hoodTalon.configNominalOutputVoltage(+0.0f, -0.0f); hoodTalon.configPeakOutputVoltage(+6.0, -6.0); + hoodTalon.setInverted(true); + hoodTalon.setSensorPhase(true); + + hoodTalon.configForwardSoftLimitThreshold(85000); + hoodTalon.configForwardSoftLimitEnable(true); + hoodTalon.configReverseSoftLimitThreshold(0); + hoodTalon.configReverseSoftLimitEnable(true); } private void configureWheelTalon() { @@ -93,7 +105,14 @@ void configureTurretTalon() { turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); turretTalon.configNominalOutputVoltage(+0.0f, -0.0f); - turretTalon.configPeakOutputVoltage(+6.0, -6.0); + turretTalon.configPeakOutputVoltage(+2.0, -2.0); + + turretTalon.configForwardSoftLimitThreshold(6000); + turretTalon.configForwardSoftLimitEnable(true); + turretTalon.configReverseSoftLimitThreshold(-6000); + turretTalon.configReverseSoftLimitEnable(true); + + this.setTurretVBus(0.0); } @Override @@ -101,9 +120,21 @@ public void periodic() { // This method will be called once per scheduler run UpdateDashboard(); + updateHistory(); } - int debugShooter; + private static final double CAMERA_LAG = 0.150; // was .200; changing to .150 at CMP + + private void updateHistory() { + double now = Timer.getFPGATimestamp(); + headingHistory.add(now, getTurretPosition()); + } + + public double getAzimuthForCapturedImage() { + double now = Timer.getFPGATimestamp(); + double indexTime = now - CAMERA_LAG; + return headingHistory.getAzForTime(indexTime); + } private void UpdateDashboard() { SmartDashboard.putNumber("Shooter Wheel pos", shooterWheelTalon.getSelectedSensorPosition(0)); @@ -114,12 +145,15 @@ private void UpdateDashboard() { SmartDashboard.putNumber("Shooter Wheel target RPM", m_targetSpeedRPM); SmartDashboard.putNumber("Shooter Wheel Error", m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelTalon.getSelectedSensorVelocity(0))); + SmartDashboard.putNumber("Shooter Wheel Voltage", shooterWheelTalon.getMotorOutputVoltage()); SmartDashboard.putNumber("Shooter turet pos", turretTalon.getPosition()); + SmartDashboard.putNumber("Shooter turet vbus", turretTalon.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter hood pos", hoodTalon.getPosition()); SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getPosition()); - SmartDashboard.putNumber("Shooter Debug", debugShooter++); + // SmartDashboard.putNumber("Shooter Debug", debugShooter++); } public void zero() { @@ -131,6 +165,13 @@ public void zero() { * Set the absolute turret position. */ public void setTurretPositionAbs(double pos) { + if (pos < TURRET_MIN_POS) { + pos = TURRET_MIN_POS; + } + if (pos > TURRET_MAX_POS) { + pos = TURRET_MAX_POS; + } + turretTalon.set(ControlMode.Position, pos); } @@ -140,7 +181,11 @@ public void setTurretPositionAbs(double pos) { * @param pos number of encoder ticks to adjust. */ public void setTurretPositionRel(double pos) { - turretTalon.set(ControlMode.Position, getTurretPosition() + pos); + setTurretPositionAbs(getTurretPosition() + pos); + } + + public void setTurretVBus(double power) { + turretTalon.set(ControlMode.PercentOutput, power); } /** @@ -160,6 +205,10 @@ public double getHoodPosition() { return hoodTalon.getPosition(); } + public void setHoodVBus(double d) { + hoodTalon.set(ControlMode.PercentOutput, d); + } + public void setFeederSpeed(double pos) { feederTalon.set(ControlMode.PercentOutput, pos); } @@ -188,51 +237,51 @@ public double getShooterWheelTargetSpeed() { } public double getShooterWheelSpeedVBus() { - return shooterWheelTalon.getOutputVoltage(); + return shooterWheelTalon.getMotorOutputVoltage(); } //////////////////////////////////////////////////// // PidTunerObject @Override public double getP() { - return shooterWheelTalon.getP(); + return turretTalon.getP(); } @Override public double getI() { - return shooterWheelTalon.getI(); + return turretTalon.getI(); } @Override public double getD() { - return shooterWheelTalon.getD(); + return turretTalon.getD(); } @Override public double getF() { - return shooterWheelTalon.getF(); + return turretTalon.getF(); } @Override public void setP(double d) { - shooterWheelTalon.config_kP(0, d, 0); + turretTalon.config_kP(0, d, 0); } @Override public void setI(double d) { - shooterWheelTalon.config_kI(0, d, 0); + turretTalon.config_kI(0, d, 0); } @Override public void setD(double d) { - shooterWheelTalon.config_kD(0, d, 0); + turretTalon.config_kD(0, d, 0); } @Override public void setF(double d) { - shooterWheelTalon.config_kF(0, d, 0); + turretTalon.config_kF(0, d, 0); } } \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index b16c645..a55a6d6 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,34 +7,260 @@ package org.mayheminc.robot2020.subsystems; +import org.mayheminc.robot2020.RobotContainer; + +// import org.mayheminc.robot2019.Robot; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import sun.net.www.content.text.plain; + +/** + * Add your docs here. + */ public class Targeting extends SubsystemBase { - /** - * Creates a new Targeting. - */ - public Targeting() { + // Put methods for controlling this subsystem + // here. Call these from Commands. - } + // COMPETITION ROBOT NEEDS THE ONE BELOW! + // private double Y_WHEN_TARGET_AT_WALL = 0.65; // Worked fine and 0.70 - public boolean isOnTarget() { - return true; - } + // PRACTICE ROBOT NEEDS THE ONE BELOW! + // private double m_YWhenTargetAtWall= 0.75; // Worked fine and 0.70 + + // Y when hatch panel is at wall when the arm is low + private static final double Y_AT_WALL_SAFETY_MARGIN = 0.05; + + private static final double Y_WHEN_HATCH_LOW_AT_WALL = 0.75 + Y_AT_WALL_SAFETY_MARGIN; + // Y when hatch panel is at wall when the arm is high + private static final double Y_WHEN_HATCH_MID_AT_WALL = 0.50 + Y_AT_WALL_SAFETY_MARGIN; + // Y when hatch panel is at wall when the arm is high + private static final double Y_WHEN_HATCH_HIGH_AT_WALL = 0.60 + Y_AT_WALL_SAFETY_MARGIN; + + private static final double SPEED_EQ_M = -4.115; + private static final double SPEED_EQ_B = 2.244; + + // Below values are for centered arm + // private static final double CENTER_EQ_M = -0.1925; + // private static final double CENTER_EQ_B = 0.5719; + + // Below values are for arm off 2 inches to left on the practice robot + // private static final double CENTER_EQ_M_HP = -0.2964; + // private static final double CENTER_EQ_B_HP = 0.5871; + // private static final double CENTER_EQ_M_CARGO = -0.6102; + // private static final double CENTER_EQ_B_CARGO = 0.6334; + + // Below values are for competition robot based upon PineTree data + private static final double CENTER_EQ_M_HP = -0.2762; + private static final double CENTER_EQ_B_HP = 0.5563; + + // below values are guesses for competition robot; same as on practice robot + private static final double CENTER_EQ_M_CARGO = -0.6102; + private static final double CENTER_EQ_B_CARGO = 0.6334; + + // After computing a desired heading, add a "fudge" offset to correct + // empirically measured error. Found to be approx -1 degree (to shift aim 1" to + // the left) during NECMP Thursday AM practice field session, for competition + // robot. + + // heading correction offset had been 0.0 for PineTree + // changed to -1.0 for first 6 matches of NECMP + // changed to 0.0 at lunch time on Friday + private static final double HEADING_CORRECTION_OFFSET = 0.0; // was -2.0 at CMP + + private double m_desiredAzimuth; + private double[] m_target_array; + private int m_priorFrameCount; + private double m_priorFrameTime; + private double[] ARRAY_OF_NEG_ONE = { -1.0 }; + private final static double FOV_CAMERA_IN_DEGREES = 78.0; + private double m_bestY = 0.0; + private double m_bestX = 0.0; + private double tilt = 0.0; + private double area = 0.0; + + public enum TargetPosition { + LEFT_MOST, CENTER_MOST, RIGHT_MOST, CENTER_OF_RIGHT_CARGO_SHIP, CENTER_OF_LEFT_CARGO_SHIP + }; + + public enum TargetHeight { + CARGO, HATCH + }; + + private TargetPosition m_mode = TargetPosition.CENTER_MOST; + // Mode for target height + private TargetHeight m_TargetHeightMode = TargetHeight.HATCH; @Override public void periodic() { - // This method will be called once per scheduler run + update(); } - public double getBearingToTarget() { - return 0; + // TODO: make an updateSmartDashboard() method in Targeting for optimization + // TODO: clean up the content in Targeting.update() -- just too long! + public void update() { + // perform periodic update functions for the targeting capability + int latestFrameCount = (int) SmartDashboard.getNumber("frameCount", -1.0 /* default to -1 */); + if (latestFrameCount < 0) { + // an invalid number for latestFrameCount, display warning light + SmartDashboard.putBoolean("visionOK", false); + SmartDashboard.putString("visionOkDebug", "No Data"); + } else if (latestFrameCount == m_priorFrameCount) { + // have not received a new frame. If more than 1 second has elapsed since + // prior new frame, display a warning light on the SmartDashboard + if (Timer.getFPGATimestamp() > m_priorFrameTime + 1.0) { + SmartDashboard.putBoolean("visionOK", false); + SmartDashboard.putString("visionOkDebug", "Stale Data"); + } else { + // else, have an old frame, but it's not too old yet, so do nothing + } + } else { + // have received a new frame, save the time and update m_priorFrameCount + m_priorFrameTime = Timer.getFPGATimestamp(); + m_priorFrameCount = latestFrameCount; + SmartDashboard.putBoolean("visionOK", true); + SmartDashboard.putString("visionOkDebug", "Good Data"); + } + + double[] centerMostTargetArray; + // Update all of the targeting information, as follows: + // 1 - Determine if we have any valid data in the array. + // If not, set the "error" to zero, so that the robot thinks + // it is on target. + // 2 - Look through the valid data in the array to find the + // target closest to the "trueCenter" + // 3 - Use the selected target to compute the needed information + + // get the latest output from the targeting camera + m_target_array = SmartDashboard.getNumberArray("target", ARRAY_OF_NEG_ONE); + + if (m_target_array == null || m_target_array.length == 0) { + // this means the key is found, but is empty + m_bestX = 0.0; + m_bestY = 0.0; + tilt = 0.0; + area = 0.0; + m_desiredAzimuth = RobotContainer.shooter.getAzimuthForCapturedImage(); + } else if (m_target_array[0] < 0.0) { + // this means the array has no valid data. Set m_xError = 0.0 + m_bestX = 0.0; + m_bestY = 0.0; + tilt = 0.0; + area = 0.0; + m_desiredAzimuth = RobotContainer.shooter.getAzimuthForCapturedImage(); + } else { + // We have a valid data array. + // There are three different situations: + // a - We want the left-most target + // b - We want the "centered" target + // c - We want the right-most target + + // Handle each of them separately; + // we need the results in "bestXError" and "bestY" + m_bestX = m_target_array[0]; // get the x-value + m_bestY = m_target_array[1]; // get the y-value + tilt = m_target_array[2]; + area = m_target_array[3]; + + m_desiredAzimuth = findDesiredAzimuth(m_bestX, m_bestY, tilt, area); + } + + // at this point in the code, the "selected" target should be in the "best" + SmartDashboard.putNumber("m_bestX", m_bestX); + SmartDashboard.putNumber("m_bestY", m_bestY); + SmartDashboard.putNumber("tilt", tilt); + SmartDashboard.putNumber("area", area); } - public double getRangeToTarget() { - return 0; + public double getDesiredAzimuth() { + return m_desiredAzimuth + HEADING_CORRECTION_OFFSET; } - public static double convertRangeToWheelSpeed(double rangeToTarget) { - return 1000; + public double getRecommendedSpeed() { + // Returns a speed based upon the Y value + double speed; + + if (m_bestY < 0.1) { + // can't see the target, set speed to something real slow + speed = 0.2; + } else { + + // use the equation to determine the speed from m_bestY + speed = (SPEED_EQ_M * m_bestY) + SPEED_EQ_B; + + // enforce min speed of 0.30 and max speed of 0.90 + if (speed < 0.30) { + speed = 0.30; + } else if (speed > 0.90) { + speed = 0.90; + } + } + + return speed; + } + + // public boolean atWall(Autonomous.RocketHeight desiredHeight) { + // // we are at the wall when the target is lower in the field of view (bigger + // Y) + // // than the "at the wall" threshold + // switch (desiredHeight) { + // case HIGH: + // return (m_bestY >= Y_WHEN_HATCH_HIGH_AT_WALL); + // case MID: + // return (m_bestY >= Y_WHEN_HATCH_MID_AT_WALL); + // case LOW: + // return (m_bestY >= Y_WHEN_HATCH_LOW_AT_WALL); + // default: + // return (m_bestY >= Y_WHEN_HATCH_LOW_AT_WALL); + // } + // } + + public void setMode(TargetPosition modeToSet) { + // Set the mode e.g. LEFT_MOST, CENTER_MOST, RIGHT_MOST, + // CENTER_OF_RIGHT_CARGO_SHIP, CENTER_OF_LEFT_CARGO_SHIP + m_mode = modeToSet; } + + private final double CenterOfTarget_X = 0.5; + private final double TICK_PER_DEGREE = (6300.0 / 45.0); + + /** + * Return the desired turrent encoder ticks in the turret for the center of the + * target. + * + * @param X + * @param Y + * @param tilt + * @param area + * @return + */ + public double findDesiredAzimuth(double X, double Y, double tilt, double area) { + // Calulate angle error based on an X,Y + double AngleError; + double ticksError; + // double TrueCenter; + double XError; + double desiredAzimuth; + + // compute the "x error" based upon the trueCenter + XError = X - CenterOfTarget_X; + // Find the angle error + AngleError = FOV_CAMERA_IN_DEGREES * XError; + ticksError = AngleError * TICK_PER_DEGREE; + + // Convert angleError into a desired heading, using the heading history + desiredAzimuth = ticksError + RobotContainer.shooter.getAzimuthForCapturedImage(); + // Update SmartDashboard + SmartDashboard.putNumber("True Angle Error", AngleError); + SmartDashboard.putNumber("Vision Desired Azimuth", desiredAzimuth); + return desiredAzimuth; + } + + public void setTargetHeight(TargetHeight target) { + m_TargetHeightMode = target; + } + } diff --git a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java index 6a6c355..96d23b6 100644 --- a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java +++ b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -115,16 +115,16 @@ public void setPID(double wheelP, double wheelI, double wheelD, double wheelF, i } // public double getSetpoint() { - // return 0; + // return 0; // } public double getError() { return this.getClosedLoopError(0); } - public float getOutputVoltage() { - return (float) this.getMotorOutputVoltage(); - } + // public float getOutputVoltage() { + // return (float) this.getMotorOutputVoltage(); + // } int pidProfile; @@ -162,6 +162,6 @@ public void setEncPosition(int i) { } // public double get() { - // return this.getOutputCurrent(); + // return this.getOutputCurrent(); // } } From 1ce4114eb48c389aa0e45e9ba4789e264b789ac2 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Sat, 15 Feb 2020 02:10:38 -0500 Subject: [PATCH 061/121] Various minor changes to get rid of a few warnings, as well as see if Ken can actually check out, build, and check in code from his computer. --- .../commands/MagazineSetTurntable.java | 1 - .../commands/ShooterFireWhenReady.java | 2 +- .../commands/ShooterReadyAimFire.java | 1 - .../commands/TargetingIsOnTarget.java | 4 +- .../robot2020/subsystems/Shooter.java | 4 +- .../robot2020/subsystems/Targeting.java | 49 ++----------------- 6 files changed, 10 insertions(+), 51 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java index 3bb1bb6..a0de4ea 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java +++ b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java @@ -9,7 +9,6 @@ import org.mayheminc.robot2020.RobotContainer; -import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj2.command.CommandBase; public class MagazineSetTurntable extends CommandBase { diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java index bac3dbc..25eaac7 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java @@ -8,7 +8,7 @@ package org.mayheminc.robot2020.commands; import org.mayheminc.robot2020.RobotContainer; -import org.mayheminc.robot2020.subsystems.Targeting; +// import org.mayheminc.robot2020.subsystems.Targeting; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index 4a9e0d2..3235a44 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -7,7 +7,6 @@ package org.mayheminc.robot2020.commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java index cff4995..1637d57 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java @@ -7,8 +7,8 @@ package org.mayheminc.robot2020.commands; -import org.mayheminc.robot2020.RobotContainer; -import org.mayheminc.robot2020.subsystems.Targeting; +// import org.mayheminc.robot2020.RobotContainer; +// import org.mayheminc.robot2020.subsystems.Targeting; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index db0f6b8..60e2651 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -1,7 +1,7 @@ package org.mayheminc.robot2020.subsystems; import org.mayheminc.robot2020.Constants; -import org.mayheminc.robot2020.RobotContainer; +// import org.mayheminc.robot2020.RobotContainer; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -12,7 +12,7 @@ import org.mayheminc.util.History; import org.mayheminc.util.MayhemTalonSRX; -import org.mayheminc.util.PidTuner; +// import org.mayheminc.util.PidTuner; import org.mayheminc.util.PidTunerObject; public class Shooter extends SubsystemBase implements PidTunerObject { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index a55a6d6..86d5fe5 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -11,7 +11,7 @@ // import org.mayheminc.robot2019.Robot; -import edu.wpi.first.wpilibj.DriverStation; +// import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -25,51 +25,12 @@ public class Targeting extends SubsystemBase { // Put methods for controlling this subsystem // here. Call these from Commands. - // COMPETITION ROBOT NEEDS THE ONE BELOW! - // private double Y_WHEN_TARGET_AT_WALL = 0.65; // Worked fine and 0.70 - - // PRACTICE ROBOT NEEDS THE ONE BELOW! - // private double m_YWhenTargetAtWall= 0.75; // Worked fine and 0.70 - - // Y when hatch panel is at wall when the arm is low - private static final double Y_AT_WALL_SAFETY_MARGIN = 0.05; - - private static final double Y_WHEN_HATCH_LOW_AT_WALL = 0.75 + Y_AT_WALL_SAFETY_MARGIN; - // Y when hatch panel is at wall when the arm is high - private static final double Y_WHEN_HATCH_MID_AT_WALL = 0.50 + Y_AT_WALL_SAFETY_MARGIN; - // Y when hatch panel is at wall when the arm is high - private static final double Y_WHEN_HATCH_HIGH_AT_WALL = 0.60 + Y_AT_WALL_SAFETY_MARGIN; - private static final double SPEED_EQ_M = -4.115; private static final double SPEED_EQ_B = 2.244; - // Below values are for centered arm - // private static final double CENTER_EQ_M = -0.1925; - // private static final double CENTER_EQ_B = 0.5719; - - // Below values are for arm off 2 inches to left on the practice robot - // private static final double CENTER_EQ_M_HP = -0.2964; - // private static final double CENTER_EQ_B_HP = 0.5871; - // private static final double CENTER_EQ_M_CARGO = -0.6102; - // private static final double CENTER_EQ_B_CARGO = 0.6334; - - // Below values are for competition robot based upon PineTree data - private static final double CENTER_EQ_M_HP = -0.2762; - private static final double CENTER_EQ_B_HP = 0.5563; - - // below values are guesses for competition robot; same as on practice robot - private static final double CENTER_EQ_M_CARGO = -0.6102; - private static final double CENTER_EQ_B_CARGO = 0.6334; - - // After computing a desired heading, add a "fudge" offset to correct - // empirically measured error. Found to be approx -1 degree (to shift aim 1" to - // the left) during NECMP Thursday AM practice field session, for competition - // robot. - - // heading correction offset had been 0.0 for PineTree - // changed to -1.0 for first 6 matches of NECMP - // changed to 0.0 at lunch time on Friday - private static final double HEADING_CORRECTION_OFFSET = 0.0; // was -2.0 at CMP + // After computing a desired azimuth, add a "fudge" offset to correct + // empirically measured error. Offset should be in azimuth "ticks." + private static final double AZIMUTH_CORRECTION_OFFSET = 0.0; // was -2.0 at CMP private double m_desiredAzimuth; private double[] m_target_array; @@ -176,7 +137,7 @@ public void update() { } public double getDesiredAzimuth() { - return m_desiredAzimuth + HEADING_CORRECTION_OFFSET; + return m_desiredAzimuth + AZIMUTH_CORRECTION_OFFSET; } public double getRecommendedSpeed() { From e81a784cddd45e340bd9e17b8034c42a6416b19e Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Sat, 15 Feb 2020 02:41:24 -0500 Subject: [PATCH 062/121] More code cleanup - removed some warnings, removed some dead code, updated some code to use our coding conventions. --- .../mayheminc/robot2020/RobotContainer.java | 6 +- .../commands/SystemZeroIncludingGyro.java | 1 - .../robot2020/subsystems/Shooter.java | 10 ++- .../robot2020/subsystems/Targeting.java | 80 ++++++------------- 4 files changed, 32 insertions(+), 65 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 7e404f0..2719182 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -9,11 +9,11 @@ import org.mayheminc.util.*; -import edu.wpi.first.wpilibj.Compressor; +// import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.Button; +// import edu.wpi.first.wpilibj2.command.button.Button; import java.util.LinkedList; @@ -48,7 +48,7 @@ public class RobotContainer { public static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); public static final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); - private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); + // private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); /** * The container for the robot. Contains subsystems, OI devices, and commands. diff --git a/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java b/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java index d75dcfe..77879c8 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java +++ b/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java @@ -24,7 +24,6 @@ public SystemZeroIncludingGyro() { @Override public boolean runsWhenDisabled() { - // TODO Auto-generated method stub return true; } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 60e2651..1d1e971 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -21,20 +21,22 @@ public class Shooter extends SubsystemBase implements PidTunerObject { private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); private final MayhemTalonSRX feederTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_FEEDER); - private final double MAX_SPEED_RPM = 5760.0; + // private final double MAX_SPEED_RPM = 5760.0; private final double TALON_TICKS_PER_REV = 2048.0; private final double TURRET_MIN_POS = -5500.0; private final double TURRET_MAX_POS = +5500.0; + private final double SECONDS_PER_MINUTE = 60.0; + private final double HUNDRED_MS_PER_SECOND = 10.0; double m_targetSpeedRPM; History headingHistory = new History(); double convertRpmToTicksPer100ms(double rpm) { - return rpm / 60 * 2048 / 10; + return rpm / SECONDS_PER_MINUTE * TALON_TICKS_PER_REV / HUNDRED_MS_PER_SECOND; } double convertTicksPer100msToRPM(double ticks) { - return ticks * 10 / 2048 * 60; + return ticks * HUNDRED_MS_PER_SECOND / TALON_TICKS_PER_REV * SECONDS_PER_MINUTE; } /** @@ -49,7 +51,7 @@ public Shooter() { shooterWheelTalon.config_kP(0, 3.0, 0); shooterWheelTalon.config_kI(0, 0.0, 0); shooterWheelTalon.config_kD(0, 0.0, 0); - shooterWheelTalon.config_kF(0, 0.048);// 1023.0 / convertRpmToTicksPer100ms(5760), 0); + shooterWheelTalon.config_kF(0, 0.048); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); } public void init() { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index 86d5fe5..9502259 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -40,8 +40,8 @@ public class Targeting extends SubsystemBase { private final static double FOV_CAMERA_IN_DEGREES = 78.0; private double m_bestY = 0.0; private double m_bestX = 0.0; - private double tilt = 0.0; - private double area = 0.0; + private double m_tilt = 0.0; + private double m_area = 0.0; public enum TargetPosition { LEFT_MOST, CENTER_MOST, RIGHT_MOST, CENTER_OF_RIGHT_CARGO_SHIP, CENTER_OF_LEFT_CARGO_SHIP @@ -51,17 +51,12 @@ public enum TargetHeight { CARGO, HATCH }; - private TargetPosition m_mode = TargetPosition.CENTER_MOST; - // Mode for target height - private TargetHeight m_TargetHeightMode = TargetHeight.HATCH; - @Override public void periodic() { update(); } // TODO: make an updateSmartDashboard() method in Targeting for optimization - // TODO: clean up the content in Targeting.update() -- just too long! public void update() { // perform periodic update functions for the targeting capability int latestFrameCount = (int) SmartDashboard.getNumber("frameCount", -1.0 /* default to -1 */); @@ -86,14 +81,11 @@ public void update() { SmartDashboard.putString("visionOkDebug", "Good Data"); } - double[] centerMostTargetArray; // Update all of the targeting information, as follows: // 1 - Determine if we have any valid data in the array. // If not, set the "error" to zero, so that the robot thinks // it is on target. - // 2 - Look through the valid data in the array to find the - // target closest to the "trueCenter" - // 3 - Use the selected target to compute the needed information + // 2 - Use the target to compute the needed information // get the latest output from the targeting camera m_target_array = SmartDashboard.getNumberArray("target", ARRAY_OF_NEG_ONE); @@ -102,15 +94,15 @@ public void update() { // this means the key is found, but is empty m_bestX = 0.0; m_bestY = 0.0; - tilt = 0.0; - area = 0.0; + m_tilt = 0.0; + m_area = 0.0; m_desiredAzimuth = RobotContainer.shooter.getAzimuthForCapturedImage(); } else if (m_target_array[0] < 0.0) { // this means the array has no valid data. Set m_xError = 0.0 m_bestX = 0.0; m_bestY = 0.0; - tilt = 0.0; - area = 0.0; + m_tilt = 0.0; + m_area = 0.0; m_desiredAzimuth = RobotContainer.shooter.getAzimuthForCapturedImage(); } else { // We have a valid data array. @@ -123,17 +115,17 @@ public void update() { // we need the results in "bestXError" and "bestY" m_bestX = m_target_array[0]; // get the x-value m_bestY = m_target_array[1]; // get the y-value - tilt = m_target_array[2]; - area = m_target_array[3]; + m_tilt = m_target_array[2]; + m_area = m_target_array[3]; - m_desiredAzimuth = findDesiredAzimuth(m_bestX, m_bestY, tilt, area); + m_desiredAzimuth = findDesiredAzimuth(m_bestX, m_bestY, m_tilt, m_area); } // at this point in the code, the "selected" target should be in the "best" SmartDashboard.putNumber("m_bestX", m_bestX); SmartDashboard.putNumber("m_bestY", m_bestY); - SmartDashboard.putNumber("tilt", tilt); - SmartDashboard.putNumber("area", area); + SmartDashboard.putNumber("m_tilt", m_tilt); + SmartDashboard.putNumber("m_area", m_area); } public double getDesiredAzimuth() { @@ -163,30 +155,8 @@ public double getRecommendedSpeed() { return speed; } - // public boolean atWall(Autonomous.RocketHeight desiredHeight) { - // // we are at the wall when the target is lower in the field of view (bigger - // Y) - // // than the "at the wall" threshold - // switch (desiredHeight) { - // case HIGH: - // return (m_bestY >= Y_WHEN_HATCH_HIGH_AT_WALL); - // case MID: - // return (m_bestY >= Y_WHEN_HATCH_MID_AT_WALL); - // case LOW: - // return (m_bestY >= Y_WHEN_HATCH_LOW_AT_WALL); - // default: - // return (m_bestY >= Y_WHEN_HATCH_LOW_AT_WALL); - // } - // } - - public void setMode(TargetPosition modeToSet) { - // Set the mode e.g. LEFT_MOST, CENTER_MOST, RIGHT_MOST, - // CENTER_OF_RIGHT_CARGO_SHIP, CENTER_OF_LEFT_CARGO_SHIP - m_mode = modeToSet; - } - - private final double CenterOfTarget_X = 0.5; - private final double TICK_PER_DEGREE = (6300.0 / 45.0); + private final double CENTER_OF_TARGET_X = 0.5; + private final double TICKS_PER_DEGREE = (6300.0 / 45.0); /** * Return the desired turrent encoder ticks in the turret for the center of the @@ -200,28 +170,24 @@ public void setMode(TargetPosition modeToSet) { */ public double findDesiredAzimuth(double X, double Y, double tilt, double area) { // Calulate angle error based on an X,Y - double AngleError; + double angleError; double ticksError; - // double TrueCenter; - double XError; + double xError; double desiredAzimuth; - // compute the "x error" based upon the trueCenter - XError = X - CenterOfTarget_X; + // compute the "x error" based upon the center for shooting + xError = X - CENTER_OF_TARGET_X; // Find the angle error - AngleError = FOV_CAMERA_IN_DEGREES * XError; - ticksError = AngleError * TICK_PER_DEGREE; + angleError = FOV_CAMERA_IN_DEGREES * xError; + // convert the angle error into ticks + ticksError = angleError * TICKS_PER_DEGREE; - // Convert angleError into a desired heading, using the heading history + // Convert angleError into a desired azimuth, using the azimuth history desiredAzimuth = ticksError + RobotContainer.shooter.getAzimuthForCapturedImage(); // Update SmartDashboard - SmartDashboard.putNumber("True Angle Error", AngleError); + SmartDashboard.putNumber("Vision Angle Error", angleError); SmartDashboard.putNumber("Vision Desired Azimuth", desiredAzimuth); return desiredAzimuth; } - public void setTargetHeight(TargetHeight target) { - m_TargetHeightMode = target; - } - } From ada9e7d974a6c286430b717aa3461dce33e2f3f4 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Sat, 15 Feb 2020 03:24:29 -0500 Subject: [PATCH 063/121] Added a StayStill() autonomous program. Changed "DriveStraight" to drive only 100 inches. Tried to fix "DISTANCE_PER_PULSE" in Drive.java to match 2020 robot drivetrain. --- src/main/java/org/mayheminc/robot2020/RobotContainer.java | 2 +- .../robot2020/autonomousroutines/DriveStraight.java | 4 ++-- src/main/java/org/mayheminc/robot2020/subsystems/Drive.java | 5 +++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 2719182..43be7e8 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -81,7 +81,7 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - // autonomousPrograms.push(new StayStill()); + autonomousPrograms.push(new StayStill()); autonomousPrograms.push(new DriveStraight()); // autonomousPrograms.push(new TestTurret()); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java index ad4b0ae..f5bc56c 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java @@ -19,9 +19,9 @@ public class DriveStraight extends SequentialCommandGroup { public DriveStraight() { addCommands(new DriveZeroGyro()); - addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0)); + // addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0)); addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 0)); - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0)); + // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0)); // addCommands(new ParallelCommandGroup(new IntakeSetPosition(true), new // MagazineSetTurntable(0.0))); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 953f2cb..73ed440 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -53,8 +53,9 @@ public class Drive extends SubsystemBase { private static final double kToleranceDegreesPIDControl = 0.2; // Drive parameters - public static final double DISTANCE_PER_PULSE = 3.14 * 8.0 * 36 / 42 / (250 * 4); // pi * diameter * (gear ratio) / - // (counts per rev) + // pi * diameter * (pulley ratios) / (counts per rev * gearbox reduction) + public static final double DISTANCE_PER_PULSE = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); + private boolean m_closedLoopMode = false; private double m_maxWheelSpeed = 1.0; // set to 1.0 as default for "open loop" percentVBus drive private double m_voltageRampRate = 48.0; From c17f55dc6ca9691d779db333bd927d56846db27b Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Mon, 17 Feb 2020 19:09:37 -0500 Subject: [PATCH 064/121] Changes from Week Zero tournament. --- .../org/mayheminc/robot2020/RobotContainer.java | 16 ++++++++++------ .../autonomousroutines/DriveStraight.java | 4 +++- .../mayheminc/robot2020/subsystems/Shooter.java | 3 +++ 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 43be7e8..ccb7c9c 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -106,6 +106,8 @@ private void configureButtonBindings() { private void configureDriverStickButtons() { DRIVER_STICK.DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); + // DRIVER_STICK.DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new + // SystemZeroIncludingGyro()); // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); // // adjust auto parameters @@ -139,6 +141,8 @@ private void configureDriverPadButtons() { // ShooterSetTurretAbs(+5500));// about +30 degrees DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));// about -30 degrees DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2));// about +30 degrees + DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); + DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); // Debug Hood // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetHoodRel(+1000)); @@ -152,7 +156,7 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheel(0.0)); DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(2700)); - DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(0.5)); + DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(1.0)); // debug climber pistons // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ClimberSetPistons(true)); @@ -164,9 +168,9 @@ private void configureOperatorStickButtons() { } private void configureOperatorPadButtons() { - OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whenPressed(new MagazineSetTurntable(0.2)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new MagazineSetTurntable(0.2)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whenPressed(new MagazineSetTurntable(1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new MagazineSetTurntable(0.5)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // new ShooterSetWheel(1000)); @@ -188,13 +192,13 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); - OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(0.7)); + // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new + // ClimberSetWinchesPower(0.7)); OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-0.7)); // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new // MagazineSetTurntable()); - // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whenPressed(new - // ClimberSetPistons(false)); + OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whileHeld(new ChimneySetChimney(-1.0)); } /** diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java index f5bc56c..2c9f6ec 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java @@ -9,6 +9,7 @@ import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Shooter; import edu.wpi.first.wpilibj2.command.*; @@ -20,7 +21,8 @@ public DriveStraight() { addCommands(new DriveZeroGyro()); // addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 0)); + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 50, 0)); + addCommands(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0)); // addCommands(new ParallelCommandGroup(new IntakeSetPosition(true), new diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 1d1e971..521ef17 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -28,6 +28,9 @@ public class Shooter extends SubsystemBase implements PidTunerObject { private final double SECONDS_PER_MINUTE = 60.0; private final double HUNDRED_MS_PER_SECOND = 10.0; + public final static double HOOD_INITIATION_LINE_POSITION = 44000; + public final static double HOOD_TARGET_ZONE_POSITION = 5000; + double m_targetSpeedRPM; History headingHistory = new History(); From 2be49c95bbc5f9228f096f388e559fbcd6f9620a Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Fri, 21 Feb 2020 08:07:35 -0500 Subject: [PATCH 065/121] Changes from practice field on 20 Feb 2020: (1) Draft of "ShooterReadyAimFire" (2) More complete motor configuration in Shooter.java (3) better calibration of Azimuth angle from Camera Positioning (4) regular use of voltage compensation (5) enable CTRE Voltage Compensation at 11.0 volts --- .../commands/ShooterReadyAimFire.java | 8 +++++-- .../robot2020/subsystems/Shooter.java | 18 +++++++-------- .../robot2020/subsystems/Targeting.java | 22 +++++++++++++------ .../org/mayheminc/util/MayhemTalonSRX.java | 22 ++----------------- 4 files changed, 32 insertions(+), 38 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index 3235a44..770a9a7 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -22,9 +22,13 @@ public ShooterReadyAimFire() { // super(new FooCommand(), new BarCommand()); super(); addCommands(new TargetingIsOnTarget()); + addCommands(new ShooterFireWhenReady()); + addCommands(new ShooterSetFeeder(1.0)); + addCommands(new ChimneySetChimney(0.5)); addCommands(new MagazineSetTurntable(0.3)); - addCommands(new ChimneySetChimney(0.3)); - addCommands(new ParallelRaceGroup(new ShooterFireWhenReady(), new Wait(5.0))); + + addCommands(new Wait(4.0)); + addCommands(new MagazineSetTurntable(0.0)); addCommands(new ChimneySetChimney(0.0)); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 521ef17..c96c6d1 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -68,7 +68,7 @@ public void init() { private void configureFeederTalon() { feederTalon.changeControlMode(ControlMode.PercentOutput); - + feederTalon.setNeutralMode(NeutralMode.Brake); feederTalon.configNominalOutputVoltage(+0.0f, -0.0f); feederTalon.configPeakOutputVoltage(+6.0, -6.0); } @@ -80,6 +80,7 @@ private void configureHoodTalon() { hoodTalon.config_kF(0, 0.0, 0); hoodTalon.changeControlMode(ControlMode.Position); + feederTalon.setNeutralMode(NeutralMode.Coast); hoodTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); hoodTalon.configNominalOutputVoltage(+0.0f, -0.0f); @@ -93,14 +94,6 @@ private void configureHoodTalon() { hoodTalon.configReverseSoftLimitEnable(true); } - private void configureWheelTalon() { - - shooterWheelTalon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); - - shooterWheelTalon.configNominalOutputVoltage(+0.0f, -0.0f); - shooterWheelTalon.configPeakOutputVoltage(+12.0, 0.0); - } - void configureTurretTalon() { turretTalon.config_kP(0, 1.0, 0); turretTalon.config_kI(0, 0.0, 0); @@ -120,6 +113,13 @@ void configureTurretTalon() { this.setTurretVBus(0.0); } + private void configureWheelTalon() { + shooterWheelTalon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); + shooterWheelTalon.setNeutralMode(NeutralMode.Coast); + shooterWheelTalon.configNominalOutputVoltage(+0.0f, -0.0f); + shooterWheelTalon.configPeakOutputVoltage(+12.0, 0.0); + } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index 9502259..9d2a6b7 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -22,8 +22,19 @@ */ public class Targeting extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. + + // Logitech C920 Field of View Information: + // Diagonal FOV = 78.0 + // Horizontal FOV = 70.42 + // Vertical FOV = 43.3 + private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 78.0; + + // Define the "target location" to be halfway from left to right + private final double CENTER_OF_TARGET_X = 0.5; + + // Calculate ticks per degree. + // encoder ticks * turret pulley teeth / drive pulley teeth / 360 degrees + private final double TICKS_PER_DEGREE = (4096.0 * 225.0 / 18.0 / 360.0); // was 6300 / 45 private static final double SPEED_EQ_M = -4.115; private static final double SPEED_EQ_B = 2.244; @@ -37,7 +48,7 @@ public class Targeting extends SubsystemBase { private int m_priorFrameCount; private double m_priorFrameTime; private double[] ARRAY_OF_NEG_ONE = { -1.0 }; - private final static double FOV_CAMERA_IN_DEGREES = 78.0; + private double m_bestY = 0.0; private double m_bestX = 0.0; private double m_tilt = 0.0; @@ -155,9 +166,6 @@ public double getRecommendedSpeed() { return speed; } - private final double CENTER_OF_TARGET_X = 0.5; - private final double TICKS_PER_DEGREE = (6300.0 / 45.0); - /** * Return the desired turrent encoder ticks in the turret for the center of the * target. @@ -178,7 +186,7 @@ public double findDesiredAzimuth(double X, double Y, double tilt, double area) { // compute the "x error" based upon the center for shooting xError = X - CENTER_OF_TARGET_X; // Find the angle error - angleError = FOV_CAMERA_IN_DEGREES * xError; + angleError = FOV_HORIZ_CAMERA_IN_DEGREES * xError; // convert the angle error into ticks ticksError = angleError * TICKS_PER_DEGREE; diff --git a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java index 96d23b6..29b63fa 100644 --- a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java +++ b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -23,6 +23,8 @@ public MayhemTalonSRX(int deviceNumber) { this.configNominalOutputReverse(0.0, 0); this.configPeakOutputForward(1.0, 0); this.configPeakOutputReverse(-1.0, 0); + this.configVoltageCompSaturation(12.0); // "full output" scaled to 12.0V for all modes when enabled. + this.enableVoltageCompensation(true); // turn on voltage compensation this.setNeutralMode(NeutralMode.Coast); @@ -91,10 +93,6 @@ public void setFeedbackDevice(FeedbackDevice feedback) { this.configSelectedFeedbackSensor(feedback, 0, 0); } - // public void reverseSensor(boolean b) { - - // } - public void configNominalOutputVoltage(float f, float g) { this.configNominalOutputForward(f / 12.0, 1000); this.configNominalOutputReverse(g / 12.0, 1000); @@ -114,18 +112,10 @@ public void setPID(double wheelP, double wheelI, double wheelD, double wheelF, i this.config_kF(pidProfile, wheelF, 1000); } - // public double getSetpoint() { - // return 0; - // } - public double getError() { return this.getClosedLoopError(0); } - // public float getOutputVoltage() { - // return (float) this.getMotorOutputVoltage(); - // } - int pidProfile; public void setProfile(int pidSlot) { @@ -141,10 +131,6 @@ public void setVoltageRampRate(double d) { this.configClosedloopRamp(0, 0); } - // public void enableControl() { - - // } - public void setPosition(int zeroPositionCount) { this.setSelectedSensorPosition(zeroPositionCount, 0, 1000); } @@ -160,8 +146,4 @@ public double getSpeed() { public void setEncPosition(int i) { setPosition(i); } - - // public double get() { - // return this.getOutputCurrent(); - // } } From ff47b3d3cffc952f433037a9084da4727b15bc6e Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Sat, 22 Feb 2020 08:55:42 -0500 Subject: [PATCH 066/121] Changes from practice field on Friday, 21 Feb 2020. Tuned azimuth control (thresholds, FOV sizing, PID tuning, motion magic, camera lag), tweaked "drive straight" PID loop, initial autonomous shooting and "trench run" programs. Added various "TODO" comments in code; still lots of others! --- src/main/java/frc/robot/Robot.java | 6 +++ .../mayheminc/robot2020/RobotContainer.java | 26 +++++++--- .../autonomousroutines/DriveStraight.java | 4 +- .../autonomousroutines/TrenchAuto.java | 51 +++++++++++++++++++ .../commands/AirCompressorDefault.java | 2 +- .../IntakeSetPositionWithoutWaiting.java | 38 ++++++++++++++ .../commands/ShooterReadyAimFire.java | 35 ++++++++++--- .../robot2020/commands/ShooterSetFeeder.java | 4 +- .../robot2020/commands/ShooterSetWheel.java | 19 +++++-- .../commands/TargetingIsOnTarget.java | 8 ++- .../robot2020/subsystems/AirCompressor.java | 2 +- .../mayheminc/robot2020/subsystems/Drive.java | 14 ++--- .../robot2020/subsystems/Shooter.java | 27 ++++++---- .../robot2020/subsystems/Targeting.java | 3 +- 14 files changed, 198 insertions(+), 41 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/IntakeSetPositionWithoutWaiting.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index df67473..2308172 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,9 +7,12 @@ package frc.robot; +import java.util.Date; + import org.mayheminc.robot2020.RobotContainer; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -35,6 +38,9 @@ public void robotInit() { // and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + + // TODO: Improve below to display something like "Code Last Loaded: 7 minutes ago" + SmartDashboard.putString("Robot Loaded on:", new Date().toLocaleString()); } /** diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index ccb7c9c..d074ccf 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -80,9 +80,12 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - - autonomousPrograms.push(new StayStill()); - autonomousPrograms.push(new DriveStraight()); + // TODO: fix "wierdness" with auto program selection - sometimes doesn't seem to work + // TODO: fix so that auto program is shown not just when changed (as shows old setting sometimes) + + // autonomousPrograms.push(/* 01 */ new StayStill()); + autonomousPrograms.push(/* 00 */ new TrenchAuto()); + // autonomousPrograms.push( new ShooterReadyAimFire()); // autonomousPrograms.push(new TestTurret()); autonomous.setAutonomousPrograms(autonomousPrograms); @@ -139,11 +142,21 @@ private void configureDriverPadButtons() { // ShooterSetTurretAbs(-5500));// about -30 degrees // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new // ShooterSetTurretAbs(+5500));// about +30 degrees - DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));// about -30 degrees - DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2));// about +30 degrees + // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));// + // about -30 degrees + // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new + // ShooterSetTurretVBus(+0.2));// about +30 degrees DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new + // ShooterSetTurretRel(-200.0)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new + // ShooterSetTurretRel(+200.0)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetTurretAbs(+0.0)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new + // ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); + // Debug Hood // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetHoodRel(+1000)); // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodRel(-1000)); @@ -154,7 +167,8 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(100.0)); DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-100.0)); DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheel(0.0)); - DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(2700)); + DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(3000)); + // TODO: above hard-coded constant (3000) should be a named constant from Shooter.java DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(1.0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java index 2c9f6ec..8dfa465 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java @@ -22,10 +22,8 @@ public DriveStraight() { addCommands(new DriveZeroGyro()); // addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0)); addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 50, 0)); - addCommands(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); + addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0)); - // addCommands(new ParallelCommandGroup(new IntakeSetPosition(true), new - // MagazineSetTurntable(0.0))); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java new file mode 100644 index 0000000..bc74a6b --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Shooter; + +import edu.wpi.first.wpilibj2.command.*; + +public class TrenchAuto extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public TrenchAuto() { + + addCommands(new DriveZeroGyro()); + // first, shoot the balls that were pre-loaded + + addCommands(new IntakeSetPositionWithoutWaiting(RobotContainer.intake.PIVOT_DOWN)); + addCommands(new ShooterReadyAimFire()); + + // then, perform a 3-point turn + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 48, 160)); + + // pick up balls while heading down the trench. + addCommands(new ParallelRaceGroup( + // intake while driving down the trench + new IntakeSetRollers(-1.0), + new SequentialCommandGroup(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 132, 180), + new Wait(0.5), new DriveStraightOnHeading(-0.2, DistanceUnits.INCHES, 12, 180)))); + + // after getting all three balls, go back to shooting position + addCommands(new DriveStraightOnHeading(-0.2, DistanceUnits.INCHES, 24, 30)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0)); + addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 6, 0)); + + addCommands(new ShooterReadyAimFire()); + + // turn the wheel off now that the shooting is all done + addCommands( new ShooterSetWheel(0.0)); + + addCommands(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java index 88ee715..690c53a 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java +++ b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java @@ -24,7 +24,7 @@ public AirCompressorDefault() { @Override public void initialize() { // robot.is - RobotContainer.compressor.setCompresor(true); + RobotContainer.compressor.setCompressor(true); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPositionWithoutWaiting.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPositionWithoutWaiting.java new file mode 100644 index 0000000..e5f9c7f --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPositionWithoutWaiting.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeSetPositionWithoutWaiting extends CommandBase { + double m_position; + + /** + * Creates a new IntakeSetPositionWithoutWaiting. + */ + public IntakeSetPositionWithoutWaiting(Double position) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + m_position = position; + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.intake.setPivot(m_position); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index 770a9a7..6259397 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -7,6 +7,9 @@ package org.mayheminc.robot2020.commands; +import org.mayheminc.robot2020.subsystems.Shooter; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -21,15 +24,31 @@ public ShooterReadyAimFire() { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); super(); - addCommands(new TargetingIsOnTarget()); - addCommands(new ShooterFireWhenReady()); - addCommands(new ShooterSetFeeder(1.0)); - addCommands(new ChimneySetChimney(0.5)); - addCommands(new MagazineSetTurntable(0.3)); - addCommands(new Wait(4.0)); + addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); + // addCommands(new ShooterSetWheel(3000.0, true)); + + // addCommands(new Wait(3.0)); + + // TODO: Add compressor control so that compressor is turned off while actively shooting. + + // aim to the target until we are on target. + addCommands( + new ParallelRaceGroup( + new ParallelCommandGroup(new TargetingIsOnTarget(), new ShooterSetWheel(3000.0, true)), + new TurretAimToTarget())); + + // turn on the feeder, wiat 0.1, turn on the Chimney, wait 0.1, turn on the magazine, shoot for about 4 seconds + addCommands(new ParallelRaceGroup( + new ShooterSetFeeder(1.0), + new SequentialCommandGroup(new Wait(0.1), new ChimneySetChimney(0.5)), + new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)), + new Wait(6.0))); - addCommands(new MagazineSetTurntable(0.0)); - addCommands(new ChimneySetChimney(0.0)); + addCommands(new ParallelRaceGroup( + new MagazineSetTurntable(0.0), + new ChimneySetChimney(0.0), + new ShooterSetFeeder(0.0), + new Wait(0.1))); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java index 0c6a044..5889aab 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java @@ -32,7 +32,9 @@ public void initialize() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - RobotContainer.shooter.setFeederSpeed(0.0); + + RobotContainer.shooter.setFeederSpeed(0.0); + } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java index 4acf4c0..4899986 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java @@ -13,15 +13,21 @@ public class ShooterSetWheel extends CommandBase { double m_rpm; + boolean m_waitForSpeed; + + public ShooterSetWheel(double rpm) { + this(rpm, false); + } /** * Creates a new ShooterSetWheel. */ - public ShooterSetWheel(double rpm) { + public ShooterSetWheel(double rpm, boolean wait) { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); + // addRequirements(RobotContainer.shooter); m_rpm = rpm; + m_waitForSpeed = wait; } // Called when the command is initially scheduled. @@ -33,6 +39,13 @@ public void initialize() { // Returns true when the command should end. @Override public boolean isFinished() { - return true; + if( m_waitForSpeed) + { + return (Math.abs( m_rpm - RobotContainer.shooter.getShooterWheelSpeed() ) < 100); + } + else + { + return true; + } } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java index 1637d57..b8e3e37 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java @@ -7,6 +7,8 @@ package org.mayheminc.robot2020.commands; +import org.mayheminc.robot2020.RobotContainer; + // import org.mayheminc.robot2020.RobotContainer; // import org.mayheminc.robot2020.subsystems.Targeting; @@ -50,6 +52,10 @@ public boolean isFinished() { // Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget()) // - RobotContainer.shooter.getShooterWheelSpeed()) < 100; // return bearingGood && wheelsGood; - return true; + + double targetPos = RobotContainer.targeting.getDesiredAzimuth(); + double turretPos = RobotContainer.shooter.getTurretPosition(); + + return ( Math.abs( targetPos - turretPos) < 50); } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java index 6b915cd..ad1fb20 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java @@ -19,7 +19,7 @@ public class AirCompressor extends SubsystemBase { public AirCompressor() { } - public void setCompresor(boolean b) { + public void setCompressor(boolean b) { // b = false; if (b) { compressor.start(); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 73ed440..5c01eff 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -48,13 +48,13 @@ public class Drive extends SubsystemBase { // NavX parameters private double m_desiredHeading = 0.0; private boolean m_useHeadingCorrection = true; - private static final double HEADING_PID_P_FOR_HIGH_GEAR = 0.030; - private static final double HEADING_PID_P_FOR_LOW_GEAR = HEADING_PID_P_FOR_HIGH_GEAR / 2.0; + private static final double HEADING_PID_P_FOR_HIGH_GEAR = 0.007; // was 0.030 in 2019 + private static final double HEADING_PID_P_FOR_LOW_GEAR = HEADING_PID_P_FOR_HIGH_GEAR; private static final double kToleranceDegreesPIDControl = 0.2; // Drive parameters // pi * diameter * (pulley ratios) / (counts per rev * gearbox reduction) - public static final double DISTANCE_PER_PULSE = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); + public static final double DISTANCE_PER_PULSE = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected for 2020 private boolean m_closedLoopMode = false; private double m_maxWheelSpeed = 1.0; // set to 1.0 as default for "open loop" percentVBus drive @@ -262,12 +262,12 @@ public void setHeadingCorrectionMode(boolean useHeadingCorrection) { private void resetAndEnableHeadingPID() { // if (Robot.shifter.getGear() == Shifter.HIGH_GEAR) { - // m_HeadingPid.setP(HEADING_PID_P_FOR_HIGH_GEAR); + m_HeadingPid.setP(HEADING_PID_P_FOR_HIGH_GEAR); // } else - { + // { // low gear - m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR); - } + // m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR); + // } m_HeadingPid.reset(); m_HeadingPid.enable(); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index c96c6d1..4cb92de 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -28,8 +28,9 @@ public class Shooter extends SubsystemBase implements PidTunerObject { private final double SECONDS_PER_MINUTE = 60.0; private final double HUNDRED_MS_PER_SECOND = 10.0; - public final static double HOOD_INITIATION_LINE_POSITION = 44000; public final static double HOOD_TARGET_ZONE_POSITION = 5000; + public final static double HOOD_INITIATION_LINE_POSITION = 65000; + public final static double HOOD_TRENCH_MID_POSITION = 80000; double m_targetSpeedRPM; History headingHistory = new History(); @@ -84,11 +85,11 @@ private void configureHoodTalon() { hoodTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); hoodTalon.configNominalOutputVoltage(+0.0f, -0.0f); - hoodTalon.configPeakOutputVoltage(+6.0, -6.0); + hoodTalon.configPeakOutputVoltage(+12.0, -12.0); hoodTalon.setInverted(true); hoodTalon.setSensorPhase(true); - hoodTalon.configForwardSoftLimitThreshold(85000); + hoodTalon.configForwardSoftLimitThreshold(100000); hoodTalon.configForwardSoftLimitEnable(true); hoodTalon.configReverseSoftLimitThreshold(0); hoodTalon.configReverseSoftLimitEnable(true); @@ -100,10 +101,16 @@ void configureTurretTalon() { turretTalon.config_kD(0, 0.0, 0); turretTalon.config_kF(0, 0.0, 0); turretTalon.changeControlMode(ControlMode.Position); + turretTalon.setNeutralMode(NeutralMode.Coast); turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + turretTalon.configMotionCruiseVelocity(800); // measured velocity of ~100K + // at 80%; set cruise to that + turretTalon.configMotionAcceleration(3200); // acceleration of 4x velocity + // allows cruise in 1/4 second + turretTalon.configNominalOutputVoltage(+0.0f, -0.0f); - turretTalon.configPeakOutputVoltage(+2.0, -2.0); + turretTalon.configPeakOutputVoltage(+4.0, -4.0); turretTalon.configForwardSoftLimitThreshold(6000); turretTalon.configForwardSoftLimitEnable(true); @@ -128,7 +135,8 @@ public void periodic() { updateHistory(); } - private static final double CAMERA_LAG = 0.150; // was .200; changing to .150 at CMP + // KBS: tuned below at practice field on 21 Feb 2020 via successive refinement. + private static final double CAMERA_LAG = 0.08; // .05 was best so far in 2020; used .150 in 2019 private void updateHistory() { double now = Timer.getFPGATimestamp(); @@ -152,8 +160,9 @@ private void UpdateDashboard() { m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelTalon.getSelectedSensorVelocity(0))); SmartDashboard.putNumber("Shooter Wheel Voltage", shooterWheelTalon.getMotorOutputVoltage()); - SmartDashboard.putNumber("Shooter turet pos", turretTalon.getPosition()); - SmartDashboard.putNumber("Shooter turet vbus", turretTalon.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter turret pos", turretTalon.getPosition()); + SmartDashboard.putNumber("Shooter turret vbus", turretTalon.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter turret velocity", turretTalon.getSelectedSensorVelocity(0)); SmartDashboard.putNumber("Shooter hood pos", hoodTalon.getPosition()); SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getPosition()); @@ -176,8 +185,8 @@ public void setTurretPositionAbs(double pos) { if (pos > TURRET_MAX_POS) { pos = TURRET_MAX_POS; } - - turretTalon.set(ControlMode.Position, pos); + // turretTalon.set(ControlMode.Position, pos); + turretTalon.set(ControlMode.MotionMagic, pos); } /** diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index 9d2a6b7..d9a57be 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -27,7 +27,8 @@ public class Targeting extends SubsystemBase { // Diagonal FOV = 78.0 // Horizontal FOV = 70.42 // Vertical FOV = 43.3 - private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 78.0; + // NOTE: 76.5 horizontal FOV determined empirically by Ken on 2/22/2020 + private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 76.5; // Define the "target location" to be halfway from left to right private final double CENTER_OF_TARGET_X = 0.5; From cd9d9aa60b696c092671acd28ca556ea7a9bfb3a Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Sun, 23 Feb 2020 08:21:39 -0500 Subject: [PATCH 067/121] Changes from Saturday, 22 Feb at Mayhem and at field: (1) Added second shooter wheel Falcon. (2) Switched PID Tuner back to shooter. (3) Code changes for removal of climber ratchet. (It works!) --- .../org/mayheminc/robot2020/Constants.java | 9 +- .../mayheminc/robot2020/RobotContainer.java | 11 +-- .../robot2020/subsystems/Climber.java | 28 +++--- .../mayheminc/robot2020/subsystems/Drive.java | 1 + .../robot2020/subsystems/Shooter.java | 86 ++++++++++++------- 5 files changed, 80 insertions(+), 55 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index b29f2b4..e7c41cd 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -20,12 +20,13 @@ public final class Constants { public final class Talon { - public static final int DRIVE_RIGHT_A = 3; // high current - public static final int DRIVE_RIGHT_B = 4; // high current public static final int DRIVE_LEFT_A = 1; // high current public static final int DRIVE_LEFT_B = 2; // high current + public static final int DRIVE_RIGHT_A = 3; // high current + public static final int DRIVE_RIGHT_B = 4; // high current - public static final int SHOOTER_WHEEL = 5; // high current + public static final int SHOOTER_WHEEL_LEFT = 5; // high current + public static final int SHOOTER_WHEEL_RIGHT = 17; // high current public static final int SHOOTER_FEEDER = 6; public static final int SHOOTER_TURRET = 7; public static final int SHOOTER_HOOD = 8; @@ -41,7 +42,7 @@ public final class Talon { public static final int CLIMBER_WALKER_LEFT = 15; public static final int CLIMBER_WALKER_RIGHT = 16; - public static final int CONTROL_PANEL_ROLLER = 17; // WON"T FIT!!! + public static final int CONTROL_PANEL_ROLLER = 18; // WON"T FIT!!! } public final class Solenoid { diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index d074ccf..8076858 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -62,7 +62,7 @@ public RobotContainer() { pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN, - RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, intake); + RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, shooter); cameraLights.set(true); } @@ -166,16 +166,12 @@ private void configureDriverPadButtons() { // Debug shooter pid velocity DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(100.0)); DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-100.0)); - DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheel(0.0)); + DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheelVBus(0.0)); DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(3000)); // TODO: above hard-coded constant (3000) should be a named constant from Shooter.java DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(1.0)); - // debug climber pistons - // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ClimberSetPistons(true)); - // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ClimberSetPistons(false)); - } private void configureOperatorStickButtons() { @@ -206,8 +202,7 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); - // OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new - // ClimberSetWinchesPower(0.7)); + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(0.7)); OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-0.7)); // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 3076a9f..3d7cb5c 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -20,8 +20,8 @@ public class Climber extends SubsystemBase { private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT); private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT); - private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); - private final MayhemTalonSRX walkerRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); + // private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); + // private final MayhemTalonSRX walkerRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); private final Solenoid pistons = new Solenoid(Constants.Solenoid.CLIMBER_PISTONS); @@ -37,13 +37,13 @@ public Climber() { winchRight.configNominalOutputVoltage(+0.0f, -0.0f); winchRight.configPeakOutputVoltage(+12.0, -12.0); - walkerRight.setNeutralMode(NeutralMode.Brake); - walkerRight.configNominalOutputVoltage(+0.0f, -0.0f); - walkerRight.configPeakOutputVoltage(+12.0, -12.0); + // walkerRight.setNeutralMode(NeutralMode.Brake); + // walkerRight.configNominalOutputVoltage(+0.0f, -0.0f); + // walkerRight.configPeakOutputVoltage(+12.0, -12.0); - walkerLeft.setNeutralMode(NeutralMode.Brake); - walkerLeft.configNominalOutputVoltage(+0.0f, -0.0f); - walkerLeft.configPeakOutputVoltage(+12.0, -12.0); + // walkerLeft.setNeutralMode(NeutralMode.Brake); + // walkerLeft.configNominalOutputVoltage(+0.0f, -0.0f); + // walkerLeft.configPeakOutputVoltage(+12.0, -12.0); } @@ -70,13 +70,13 @@ public void setWinchRightSpeed(double power) { winchRight.set(ControlMode.PercentOutput, power); } - public void setWalkerLeftSpeed(double power) { - walkerLeft.set(ControlMode.Velocity, power); - } + // public void setWalkerLeftSpeed(double power) { + // walkerLeft.set(ControlMode.Velocity, power); + // } - public void setWalkerRightSpeed(double power) { - walkerRight.set(ControlMode.Velocity, power); - } + // public void setWalkerRightSpeed(double power) { + // walkerRight.set(ControlMode.Velocity, power); + // } public void setPistons(boolean b) { pistons.set(b); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 5c01eff..8eba99a 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -27,6 +27,7 @@ public class Drive extends SubsystemBase { public static final boolean COAST_MODE = false; // PID loop variables + // TODO: Suppress 'deprecated' warnings from the compiler private PIDController m_HeadingPid; private PIDHeadingError m_HeadingError; private PIDHeadingCorrection m_HeadingCorrection; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 4cb92de..5432538 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -16,7 +16,8 @@ import org.mayheminc.util.PidTunerObject; public class Shooter extends SubsystemBase implements PidTunerObject { - private final MayhemTalonSRX shooterWheelTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL); + private final MayhemTalonSRX shooterWheelLeft = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_LEFT); + private final MayhemTalonSRX shooterWheelRight = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_RIGHT); private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); private final MayhemTalonSRX feederTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_FEEDER); @@ -47,20 +48,24 @@ public class Shooter extends SubsystemBase implements PidTunerObject { * Creates a new Shooter. */ public Shooter() { + configureWheelFalcons(); configureTurretTalon(); - configureWheelTalon(); configureHoodTalon(); configureFeederTalon(); - shooterWheelTalon.config_kP(0, 3.0, 0); - shooterWheelTalon.config_kI(0, 0.0, 0); - shooterWheelTalon.config_kD(0, 0.0, 0); - shooterWheelTalon.config_kF(0, 0.048); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); + shooterWheelLeft.config_kP(0, 3.0, 0); + shooterWheelLeft.config_kI(0, 0.0, 0); + shooterWheelLeft.config_kD(0, 0.0, 0); + shooterWheelLeft.config_kF(0, 0.046); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); + + // set right Falcon to follow the left Falcon + shooterWheelRight.changeControlMode(ControlMode.Follower); + shooterWheelRight.set(shooterWheelLeft.getDeviceID()); } public void init() { + configureWheelFalcons(); configureTurretTalon(); - configureWheelTalon(); configureHoodTalon(); configureFeederTalon(); @@ -120,11 +125,20 @@ void configureTurretTalon() { this.setTurretVBus(0.0); } - private void configureWheelTalon() { - shooterWheelTalon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); - shooterWheelTalon.setNeutralMode(NeutralMode.Coast); - shooterWheelTalon.configNominalOutputVoltage(+0.0f, -0.0f); - shooterWheelTalon.configPeakOutputVoltage(+12.0, 0.0); + private void configureWheelFalcons() { + shooterWheelLeft.setFeedbackDevice(FeedbackDevice.IntegratedSensor); + shooterWheelLeft.setNeutralMode(NeutralMode.Coast); + shooterWheelLeft.configNominalOutputVoltage(+0.0f, -0.0f); + shooterWheelLeft.configPeakOutputVoltage(+12.0, 0.0); + shooterWheelLeft.setInverted(false); + shooterWheelLeft.setSensorPhase(false); + + shooterWheelRight.setFeedbackDevice(FeedbackDevice.IntegratedSensor); + shooterWheelRight.setNeutralMode(NeutralMode.Coast); + shooterWheelRight.configNominalOutputVoltage(+0.0f, -0.0f); + shooterWheelRight.configPeakOutputVoltage(+12.0, 0.0); + shooterWheelRight.setInverted(true); + shooterWheelRight.setSensorPhase(true); } @Override @@ -150,15 +164,29 @@ public double getAzimuthForCapturedImage() { } private void UpdateDashboard() { - SmartDashboard.putNumber("Shooter Wheel pos", shooterWheelTalon.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelTalon.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Shooter Wheel pos", shooterWheelLeft.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelLeft.getSelectedSensorVelocity(0)); SmartDashboard.putNumber("Shooter Wheel RPM", - convertTicksPer100msToRPM(shooterWheelTalon.getSelectedSensorVelocity(0))); + convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0))); SmartDashboard.putNumber("Shooter Wheel target RPM", m_targetSpeedRPM); SmartDashboard.putNumber("Shooter Wheel Error", - m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelTalon.getSelectedSensorVelocity(0))); - SmartDashboard.putNumber("Shooter Wheel Voltage", shooterWheelTalon.getMotorOutputVoltage()); + m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0))); + SmartDashboard.putNumber("Shooter Wheel Voltage", shooterWheelLeft.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter Wheel Bus Voltage", shooterWheelLeft.getBusVoltage()); + SmartDashboard.putNumber("Shooter Wheel Current", shooterWheelLeft.getSupplyCurrent()); + + SmartDashboard.putNumber("Shooter Wheel R-pos", shooterWheelRight.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Shooter Wheel R-speed", shooterWheelRight.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Shooter Wheel R-RPM", + convertTicksPer100msToRPM(shooterWheelRight.getSelectedSensorVelocity(0))); + + SmartDashboard.putNumber("Shooter Wheel R-target RPM", m_targetSpeedRPM); + SmartDashboard.putNumber("Shooter Wheel R-Error", + m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelRight.getSelectedSensorVelocity(0))); + SmartDashboard.putNumber("Shooter Wheel R-Voltage", shooterWheelRight.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter Wheel R-Bus Voltage", shooterWheelRight.getBusVoltage()); + SmartDashboard.putNumber("Shooter Wheel R-Current", shooterWheelRight.getSupplyCurrent()); SmartDashboard.putNumber("Shooter turret pos", turretTalon.getPosition()); SmartDashboard.putNumber("Shooter turret vbus", turretTalon.getMotorOutputVoltage()); @@ -235,15 +263,15 @@ public void setFeederSpeed(double pos) { public void setShooterWheelSpeed(double rpm) { double ticks = convertRpmToTicksPer100ms(rpm); m_targetSpeedRPM = rpm; - shooterWheelTalon.set(ControlMode.Velocity, ticks); + shooterWheelLeft.set(ControlMode.Velocity, ticks); } public void setShooterWheelSpeedVBus(double pos) { - shooterWheelTalon.set(ControlMode.PercentOutput, pos); + shooterWheelLeft.set(ControlMode.PercentOutput, pos); } public double getShooterWheelSpeed() { - return convertTicksPer100msToRPM(shooterWheelTalon.getSelectedSensorVelocity(0)); + return convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0)); } public double getShooterWheelTargetSpeed() { @@ -251,51 +279,51 @@ public double getShooterWheelTargetSpeed() { } public double getShooterWheelSpeedVBus() { - return shooterWheelTalon.getMotorOutputVoltage(); + return shooterWheelLeft.getMotorOutputVoltage(); } //////////////////////////////////////////////////// // PidTunerObject @Override public double getP() { - return turretTalon.getP(); + return shooterWheelLeft.getP(); } @Override public double getI() { - return turretTalon.getI(); + return shooterWheelLeft.getI(); } @Override public double getD() { - return turretTalon.getD(); + return shooterWheelLeft.getD(); } @Override public double getF() { - return turretTalon.getF(); + return shooterWheelLeft.getF(); } @Override public void setP(double d) { - turretTalon.config_kP(0, d, 0); + shooterWheelLeft.config_kP(0, d, 0); } @Override public void setI(double d) { - turretTalon.config_kI(0, d, 0); + shooterWheelLeft.config_kI(0, d, 0); } @Override public void setD(double d) { - turretTalon.config_kD(0, d, 0); + shooterWheelLeft.config_kD(0, d, 0); } @Override public void setF(double d) { - turretTalon.config_kF(0, d, 0); + shooterWheelLeft.config_kF(0, d, 0); } } \ No newline at end of file From ac70bfe6deebb6ddb881e221adcc53445fbf93c3 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Sun, 23 Feb 2020 09:05:52 -0500 Subject: [PATCH 068/121] Added @SuppressWarnings("removal") annotation to Robot.java, Drive.java, PIDHeadingCorrection.java, and PIDHeadingError.java in order to suppress warnings related to some of the WPILib PIDController classes being deprecated. Need to remember to fix these in Summer 2020, so also added a "TODO" to the code to fix these later. --- src/main/java/frc/robot/Robot.java | 1 + .../java/org/mayheminc/robot2020/subsystems/Drive.java | 10 +++++----- .../robot2020/subsystems/PIDHeadingCorrection.java | 2 +- .../robot2020/subsystems/PIDHeadingError.java | 1 + 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2308172..607473a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -33,6 +33,7 @@ public class Robot extends TimedRobot { * for any initialization code. */ @Override + @SuppressWarnings("deprecation") public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, // and put our diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 8eba99a..54b1ffb 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -18,6 +18,8 @@ import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.Utils; +// TODO: Address all deprecated code masked by @SuppressWarnings("removal") annotations (not just in Drive.java) +@SuppressWarnings("removal") public class Drive extends SubsystemBase { History headingHistory = new History(); @@ -27,7 +29,6 @@ public class Drive extends SubsystemBase { public static final boolean COAST_MODE = false; // PID loop variables - // TODO: Suppress 'deprecated' warnings from the compiler private PIDController m_HeadingPid; private PIDHeadingError m_HeadingError; private PIDHeadingCorrection m_HeadingCorrection; @@ -49,8 +50,7 @@ public class Drive extends SubsystemBase { // NavX parameters private double m_desiredHeading = 0.0; private boolean m_useHeadingCorrection = true; - private static final double HEADING_PID_P_FOR_HIGH_GEAR = 0.007; // was 0.030 in 2019 - private static final double HEADING_PID_P_FOR_LOW_GEAR = HEADING_PID_P_FOR_HIGH_GEAR; + private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR private static final double kToleranceDegreesPIDControl = 0.2; // Drive parameters @@ -94,7 +94,7 @@ public Drive() { m_HeadingError = new PIDHeadingError(); m_HeadingError.m_Error = 0.0; m_HeadingCorrection = new PIDHeadingCorrection(); - m_HeadingPid = new PIDController(HEADING_PID_P_FOR_HIGH_GEAR, 0.000, 0.04, m_HeadingError, m_HeadingCorrection, + m_HeadingPid = new PIDController(HEADING_PID_P, 0.000, 0.04, m_HeadingError, m_HeadingCorrection, 0.020 /* period in seconds */); m_HeadingPid.setInputRange(-180.0f, 180.0f); m_HeadingPid.setContinuous(true); // treats the input range as "continous" with wrap-around @@ -263,7 +263,7 @@ public void setHeadingCorrectionMode(boolean useHeadingCorrection) { private void resetAndEnableHeadingPID() { // if (Robot.shifter.getGear() == Shifter.HIGH_GEAR) { - m_HeadingPid.setP(HEADING_PID_P_FOR_HIGH_GEAR); + m_HeadingPid.setP(HEADING_PID_P); // } else // { // low gear diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java index 809cec7..bcf782e 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java @@ -1,5 +1,4 @@ package org.mayheminc.robot2020.subsystems; - import edu.wpi.first.wpilibj.PIDOutput; /** @@ -7,6 +6,7 @@ * @author user This class holds the correction that is calculated by the PID * Controller. */ +@SuppressWarnings("removal") public class PIDHeadingCorrection implements PIDOutput { public double HeadingCorrection = 0.0; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java index 7d1bf02..8df40c6 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java @@ -7,6 +7,7 @@ * * @author user This is a class to hold the Heading error of the drive. */ +@SuppressWarnings("removal") public class PIDHeadingError implements PIDSource { @Override From 4b6d321a8b6c9475b6d664fe7c2eb0d3e2e89286 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Sun, 23 Feb 2020 09:24:01 -0500 Subject: [PATCH 069/121] Changed climber controls to use "setInverted()" for left motor, rather than simply negating power commands in "ClimberSetWinchesPower()" command. --- .../robot2020/commands/ClimberSetWinchesPower.java | 2 +- .../java/org/mayheminc/robot2020/subsystems/Climber.java | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java index d753739..d8536b5 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java @@ -31,7 +31,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - RobotContainer.climber.setWinchLeftSpeed(-power); + RobotContainer.climber.setWinchLeftSpeed(power); RobotContainer.climber.setWinchRightSpeed(power); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 3d7cb5c..27ffcad 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -29,14 +29,20 @@ public class Climber extends SubsystemBase { * Creates a new Climber. */ public Climber() { + + // TODO: Add "soft limit" to top of climber travel to avoid "overwinding" + // TODO: Add position control to go all the way to the top, and nearly all the way in for climbing + winchLeft.setNeutralMode(NeutralMode.Brake); winchLeft.configNominalOutputVoltage(+0.0f, -0.0f); winchLeft.configPeakOutputVoltage(+12.0, -12.0); + winchLeft.setInverted(true); winchRight.setNeutralMode(NeutralMode.Brake); winchRight.configNominalOutputVoltage(+0.0f, -0.0f); winchRight.configPeakOutputVoltage(+12.0, -12.0); - + winchRight.setInverted(false); + // walkerRight.setNeutralMode(NeutralMode.Brake); // walkerRight.configNominalOutputVoltage(+0.0f, -0.0f); // walkerRight.configPeakOutputVoltage(+12.0, -12.0); From a969116b037c3b4f6bc46fd575f840a4334f5ae6 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Mon, 24 Feb 2020 20:55:43 -0500 Subject: [PATCH 070/121] Re-scaled getThrottle() and getSteering() for increased sensitivity. Added a SpeedRacerDriveNew for closed loop testing. Increased Climb Winch power to full power. Added soft limits for climber. Added ShooterAdjustHood() command. Removed some old methods from MayhemTalonSRX. Various minor naming fixes. --- src/main/java/frc/robot/Robot.java | 6 +- .../mayheminc/robot2020/RobotContainer.java | 23 ++- .../commands/DriveStraightOnHeading.java | 2 +- .../robot2020/commands/ShooterAdjustHood.java | 39 +++++ .../robot2020/subsystems/AirCompressor.java | 2 + .../robot2020/subsystems/Climber.java | 12 +- .../mayheminc/robot2020/subsystems/Drive.java | 146 ++++++++++++------ .../robot2020/subsystems/Intake.java | 5 +- .../robot2020/subsystems/Shooter.java | 56 ++++--- .../org/mayheminc/util/MayhemDriverPad.java | 61 ++++++-- .../org/mayheminc/util/MayhemTalonSRX.java | 23 --- 11 files changed, 259 insertions(+), 116 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustHood.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 607473a..b37972a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -35,12 +35,10 @@ public class Robot extends TimedRobot { @Override @SuppressWarnings("deprecation") public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, - // and put our - // autonomous chooser on the dashboard. + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - // TODO: Improve below to display something like "Code Last Loaded: 7 minutes ago" SmartDashboard.putString("Robot Loaded on:", new Date().toLocaleString()); } diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 8076858..dbfbdca 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -75,6 +75,15 @@ private void configureDefaultCommands() { drive.setDefaultCommand(new DriveDefault()); // intake.setDefaultCommand(new IntakeExtenderVBus()); magazine.setDefaultCommand(new MagazineDefault()); + + // TODO: Figure out if the current approach of "AirCompressorDefault()" is the way to go for compressor control. + // KBS doesn't think the below is the right way to have the compressor be on "by default" because + // it would require there to always be a command running to keep the compressor off. However, that + // is a good way to ensure it doesn't get left off by accident. Not quite sure how to handle this; + // would really rather that other commands which need the compressor off (such as a high-power command + // which wants all the battery power available) would turn the compressor off when the command starts + // and off when the command ends.) Then again, maybe the "defaultCommand" is a good way to do this + // and I just don't understand the style yet. compressor.setDefaultCommand(new AirCompressorDefault()); } @@ -188,22 +197,24 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whileHeld(new TurretAimToTarget()); - + OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(true)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_TEN.whenPressed(new ClimberSetPistons(false)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); + // OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new // IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterAdjustHood(+1000.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterAdjustHood(-1000.0)); - OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(0.7)); - OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-0.7)); + // TODO: Consider if below should use "variable power" for climber or just always full speed? + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(1.0)); + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-1.0)); // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new // MagazineSetTurntable()); diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java index dca08b3..9f73bb3 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java @@ -35,7 +35,7 @@ public DriveStraightOnHeading(double arg_targetSpeed, DistanceUnits units, doubl addRequirements(RobotContainer.drive); if (units == DistanceUnits.INCHES) { - arg_distance = arg_distance / Drive.DISTANCE_PER_PULSE; + arg_distance = arg_distance / Drive.DISTANCE_PER_PULSE_IN_INCHES; } m_targetPower = arg_targetSpeed; m_desiredDisplacement = Math.abs(arg_distance); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustHood.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustHood.java new file mode 100644 index 0000000..99d1bef --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustHood.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterAdjustHood extends CommandBase { + + double m_adjust; + + /** + * Creates a new ShooterAdjustHood. + */ + public ShooterAdjustHood(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooter); + + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooter.setHoodPosition(RobotContainer.shooter.getHoodPosition() + m_adjust); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java index ad1fb20..fc1064e 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java @@ -17,6 +17,8 @@ public class AirCompressor extends SubsystemBase { * Creates a new compressor. */ public AirCompressor() { + // turn on the compressor in the constructor + setCompressor(true); } public void setCompressor(boolean b) { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 27ffcad..21df5a4 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -18,6 +18,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { + private static final int MAX_HEIGHT_SOFT_LIMIT = 640000; + private static final int MIN_HEIGHT_SOFT_LIMIT = 5000; + private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT); private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT); // private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); @@ -37,11 +40,19 @@ public Climber() { winchLeft.configNominalOutputVoltage(+0.0f, -0.0f); winchLeft.configPeakOutputVoltage(+12.0, -12.0); winchLeft.setInverted(true); + winchLeft.configForwardSoftLimitThreshold(MAX_HEIGHT_SOFT_LIMIT); + winchLeft.configForwardSoftLimitEnable(true); + winchLeft.configReverseSoftLimitThreshold(MIN_HEIGHT_SOFT_LIMIT); + winchLeft.configReverseSoftLimitEnable(true); winchRight.setNeutralMode(NeutralMode.Brake); winchRight.configNominalOutputVoltage(+0.0f, -0.0f); winchRight.configPeakOutputVoltage(+12.0, -12.0); winchRight.setInverted(false); + winchRight.configForwardSoftLimitThreshold(MAX_HEIGHT_SOFT_LIMIT); + winchRight.configForwardSoftLimitEnable(true); + winchRight.configReverseSoftLimitThreshold(MIN_HEIGHT_SOFT_LIMIT); + winchRight.configReverseSoftLimitEnable(true); // walkerRight.setNeutralMode(NeutralMode.Brake); // walkerRight.configNominalOutputVoltage(+0.0f, -0.0f); @@ -50,7 +61,6 @@ public Climber() { // walkerLeft.setNeutralMode(NeutralMode.Brake); // walkerLeft.configNominalOutputVoltage(+0.0f, -0.0f); // walkerLeft.configPeakOutputVoltage(+12.0, -12.0); - } public void zero() { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 54b1ffb..12c17cd 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -55,11 +55,11 @@ public class Drive extends SubsystemBase { // Drive parameters // pi * diameter * (pulley ratios) / (counts per rev * gearbox reduction) - public static final double DISTANCE_PER_PULSE = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected for 2020 + public static final double DISTANCE_PER_PULSE_IN_INCHES = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected for 2020 private boolean m_closedLoopMode = false; - private double m_maxWheelSpeed = 1.0; // set to 1.0 as default for "open loop" percentVBus drive - private double m_voltageRampRate = 48.0; + private double m_maxWheelSpeed = 1.0; // should be maximum wheel speed in native units + private static final double CLOSED_LOOP_RAMP_RATE = 0.1; // time from neutral to full in seconds private double m_initialWheelDistance = 0.0; private int m_iterationsSinceRotationCommanded = 0; @@ -102,17 +102,14 @@ public Drive() { m_HeadingPid.setAbsoluteTolerance(kToleranceDegreesPIDControl); // confirm all four drive talons are in coast mode - leftFrontTalon.setNeutralMode(NeutralMode.Coast); - leftRearTalon.setNeutralMode(NeutralMode.Coast); - rightFrontTalon.setNeutralMode(NeutralMode.Coast); - rightRearTalon.setNeutralMode(NeutralMode.Coast); + leftFrontTalon.setNeutralMode(NeutralMode.Brake); + leftRearTalon.setNeutralMode(NeutralMode.Brake); + rightFrontTalon.setNeutralMode(NeutralMode.Brake); + rightRearTalon.setNeutralMode(NeutralMode.Brake); // set rear talons to follow their respective front talons - leftRearTalon.changeControlMode(ControlMode.Follower); - leftRearTalon.set(leftFrontTalon.getDeviceID()); - - rightRearTalon.changeControlMode(ControlMode.Follower); - rightRearTalon.set(rightFrontTalon.getDeviceID()); + leftRearTalon.follow(leftFrontTalon); + rightRearTalon.follow(rightFrontTalon); // the left motors move the robot forwards with positive power // but the right motors are backwards. @@ -120,12 +117,6 @@ public Drive() { leftRearTalon.setInverted(false); rightFrontTalon.setInverted(true); rightRearTalon.setInverted(true); - - // sensor phase is reversed, since there are 3 reduction stages in the gearbox - leftFrontTalon.setSensorPhase(true); - leftRearTalon.setSensorPhase(true); - rightFrontTalon.setSensorPhase(true); - rightRearTalon.setSensorPhase(true); } public void init() { @@ -133,8 +124,8 @@ public void init() { zeroHeadingGyro(0.0); // talon closed loop config - configureCanTalon(leftFrontTalon); - configureCanTalon(rightFrontTalon); + configureDriveTalon(leftFrontTalon); + configureDriveTalon(rightFrontTalon); } public void zeroHeadingGyro(double headingOffset) { @@ -155,29 +146,22 @@ public void initDefaultCommand() { // setDefaultCommand(new SpeedRacerDrive()); } - private void configureCanTalon(MayhemTalonSRX talon) { + private void configureDriveTalon(MayhemTalonSRX talon) { double wheelP = 1.5; double wheelI = 0.0; double wheelD = 0.0; double wheelF = 1.0; talon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); - - // talon.reverseSensor(false); + talon.configNominalOutputVoltage(+0.0f, -0.0f); talon.configPeakOutputVoltage(+12.0, -12.0); - if (m_closedLoopMode) { - talon.changeControlMode(ControlMode.Velocity); - m_maxWheelSpeed = 270; - } else { - talon.changeControlMode(ControlMode.PercentOutput); - m_maxWheelSpeed = 1.0; - } - - talon.setPID(wheelP, wheelI, wheelD, wheelF, 0, m_voltageRampRate, 0); - - // talon.enableControl(); + talon.config_kP(0, wheelP); + talon.config_kI(0, wheelI); + talon.config_kD(0, wheelD); + talon.config_kF(0, wheelF); + talon.configClosedloopRamp(CLOSED_LOOP_RAMP_RATE); // specify minimum time for neutral to full in seconds DriverStation.reportError("setWheelPIDF: " + wheelP + " " + wheelI + " " + wheelD + " " + wheelF + "\n", false); } @@ -210,16 +194,10 @@ public void toggleClosedLoopMode() { public void setClosedLoopMode() { m_closedLoopMode = true; - // reconfigure the "master" drive talons now that we're in closed loop mode - configureCanTalon(leftFrontTalon); - configureCanTalon(rightFrontTalon); } public void setOpenLoopMode() { m_closedLoopMode = false; - // reconfigure the "master" drive talons now that we're in open loop mode - configureCanTalon(leftFrontTalon); - configureCanTalon(rightFrontTalon); } // ********************* ENCODER-GETTERS ************************************ @@ -417,6 +395,89 @@ public void setAutoAlignFalse() { } public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickTurn) { + speedRacerDriveNew(throttle, rawSteeringX, quickTurn); + } + + + public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean quickTurn) { + double leftPower, rightPower; + double rotation = 0; + final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast. + + int throttleSign; + if (throttle >= 0.0) { + throttleSign = 1; + } else { + throttleSign = -1; + } + + // check for if steering input is essentially zero for "DriveStraight" functionality + if ((-0.01 < rawSteeringX) && (rawSteeringX < 0.01)) { + // no turn being commanded, drive straight or stay still + m_iterationsSinceRotationCommanded++; + if ((-0.01 < throttle) && (throttle < 0.01)) { + // no motion commanded, stay still + m_iterationsSinceMovementCommanded++; + rotation = 0.0; + m_desiredHeading = getHeading(); // whenever motionless, set desired heading to current heading + // reset the PID controller loop now that we have a new desired heading + resetAndEnableHeadingPID(); + } else { + // driving straight + if ((m_iterationsSinceRotationCommanded == LOOPS_GYRO_DELAY) + || (m_iterationsSinceMovementCommanded >= LOOPS_GYRO_DELAY)) { + // exactly LOOPS_GYRO_DELAY iterations with no commanded turn, + // or haven't had movement commanded for longer than LOOPS_GYRO_DELAY, + // so we want to take steps to preserve our current heading hereafter + + // get current heading as desired heading + m_desiredHeading = getHeading(); + + // reset the PID controller loop now that we have a new desired heading + resetAndEnableHeadingPID(); + rotation = 0.0; + } else if (m_iterationsSinceRotationCommanded < LOOPS_GYRO_DELAY) { + // not long enough since we were last turning, + // just drive straight without special heading maintenance + rotation = 0.0; + } else if (m_iterationsSinceRotationCommanded > LOOPS_GYRO_DELAY) { + // after more then LOOPS_GYRO_DELAY iterations since commanded turn, + // maintain the target heading + rotation = maintainHeading(); + } + m_iterationsSinceMovementCommanded = 0; + } + } else { + // commanding a turn, reset iterationsSinceRotationCommanded + m_iterationsSinceRotationCommanded = 0; + m_iterationsSinceMovementCommanded = 0; + } + + if (quickTurn) { + // want a high-rate turn (also allows "spin" behavior) + // power to each wheel is a combination of the throttle and rotation + rotation = rawSteeringX * throttleSign * QUICK_TURN_GAIN; + leftPower = throttle + rotation; + rightPower = throttle - rotation; + } else { + // want a standard rate turn (scaled by the throttle) + if (rawSteeringX >= 0.0) { + // turning to the right, derate the right power by turn amount + // note that rawSteeringX is positive in this portion of the "if" + leftPower = throttle; + rightPower = throttle * (1.0 - Math.abs(rawSteeringX)); + } else { + // turning to the left, derate the left power by turn amount + // note that rawSteeringX is negative in this portion of the "if" + leftPower = throttle * (1.0 - Math.abs(rawSteeringX)); + rightPower = throttle; + } + } + setMotorPower(leftPower, rightPower); + } + + + public void speedRacerDriveOld(double throttle, double rawSteeringX, boolean quickTurn) { double leftPower, rightPower; double rotation = 0; double adjustedSteeringX = rawSteeringX * throttle; @@ -498,7 +559,6 @@ public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickT leftPower = throttle + rotation; rightPower = throttle - rotation; setMotorPower(leftPower, rightPower); - } public int stage = 0; @@ -597,9 +657,9 @@ public void updateSmartDashboard() { // b - divide by 12 (1 foot per 12 inches) // c - multiply by distance (in inches) per pulse SmartDashboard.putNumber("Left Speed (fps)", - leftFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE); + leftFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE_IN_INCHES); SmartDashboard.putNumber("Right Speed (fps)", - rightFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE); + rightFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE_IN_INCHES); SmartDashboard.putNumber("Left Talon Output Voltage", leftFrontTalon.getMotorOutputVoltage()); SmartDashboard.putNumber("Right Talon Output Voltage", rightFrontTalon.getMotorOutputVoltage()); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 6342cea..c328c64 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -47,10 +47,10 @@ public Intake() { rollerTalon.configNominalOutputVoltage(+0.0f, -0.0f); rollerTalon.configPeakOutputVoltage(+12.0, -12.0); - configMotor(pivotTalon); + configPivotMotor(pivotTalon); } - void configMotor(MayhemTalonSRX motor) { + void configPivotMotor(MayhemTalonSRX motor) { // initial calcs for computing kP... // If we want 50% power when at the full extreme, // Full extreme is 900 ticks @@ -71,6 +71,7 @@ void configMotor(MayhemTalonSRX motor) { motor.setNeutralMode(NeutralMode.Coast); motor.setInverted(false); + // in general, sensor phase inversion needed for gearboxes which reverse sensor direction due to odd number of stages motor.setSensorPhase(true); motor.configNominalOutputVoltage(+0.0f, -0.0f); motor.configPeakOutputVoltage(+12.0, -12.0); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index 5432538..c3545a9 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -36,10 +36,12 @@ public class Shooter extends SubsystemBase implements PidTunerObject { double m_targetSpeedRPM; History headingHistory = new History(); + // Note: for ease of thinking, 1 RPM =~ 3.4 native units for the shooter double convertRpmToTicksPer100ms(double rpm) { return rpm / SECONDS_PER_MINUTE * TALON_TICKS_PER_REV / HUNDRED_MS_PER_SECOND; } + // Note: 3.413 native units =~ 1.0 RPM for the shooter double convertTicksPer100msToRPM(double ticks) { return ticks * HUNDRED_MS_PER_SECOND / TALON_TICKS_PER_REV * SECONDS_PER_MINUTE; } @@ -52,15 +54,6 @@ public Shooter() { configureTurretTalon(); configureHoodTalon(); configureFeederTalon(); - - shooterWheelLeft.config_kP(0, 3.0, 0); - shooterWheelLeft.config_kI(0, 0.0, 0); - shooterWheelLeft.config_kD(0, 0.0, 0); - shooterWheelLeft.config_kF(0, 0.046); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); - - // set right Falcon to follow the left Falcon - shooterWheelRight.changeControlMode(ControlMode.Follower); - shooterWheelRight.set(shooterWheelLeft.getDeviceID()); } public void init() { @@ -125,26 +118,38 @@ void configureTurretTalon() { this.setTurretVBus(0.0); } + // configure a pair of shooter wheel falcons private void configureWheelFalcons() { - shooterWheelLeft.setFeedbackDevice(FeedbackDevice.IntegratedSensor); - shooterWheelLeft.setNeutralMode(NeutralMode.Coast); - shooterWheelLeft.configNominalOutputVoltage(+0.0f, -0.0f); - shooterWheelLeft.configPeakOutputVoltage(+12.0, 0.0); + // most of the configuration is shared for the two Falcons + configureOneWheelFalcon(shooterWheelLeft); + configureOneWheelFalcon(shooterWheelRight); + + // with the exception of one rotating the opposite direction shooterWheelLeft.setInverted(false); - shooterWheelLeft.setSensorPhase(false); - - shooterWheelRight.setFeedbackDevice(FeedbackDevice.IntegratedSensor); - shooterWheelRight.setNeutralMode(NeutralMode.Coast); - shooterWheelRight.configNominalOutputVoltage(+0.0f, -0.0f); - shooterWheelRight.configPeakOutputVoltage(+12.0, 0.0); shooterWheelRight.setInverted(true); - shooterWheelRight.setSensorPhase(true); + } + private void configureOneWheelFalcon(MayhemTalonSRX shooterWheelFalcon) { + shooterWheelFalcon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); + shooterWheelFalcon.setNeutralMode(NeutralMode.Coast); + shooterWheelFalcon.configNominalOutputVoltage(+0.0f, -0.0f); + shooterWheelFalcon.configPeakOutputVoltage(+12.0, 0.0); + shooterWheelFalcon.configNeutralDeadband(0.001); // Config neutral deadband to be the smallest possible + + // For PIDF computations, 1023 is interpreted as "full" motor output + // Velocity is in native units of TicksPer100ms. + // 1000rpm =~ 3413 native units. + // P of "3.0" means that full power applied with error of 341 native units = 100rpm + // (above also means that 50% power boost applied with error of 50rpm) + shooterWheelFalcon.config_kP(0, 0.1, 0); // previously used 3.0 + shooterWheelFalcon.config_kI(0, 0.0, 0); + shooterWheelFalcon.config_kD(0, 0.0, 0); // CTRE recommends starting at 10x P-gain + shooterWheelFalcon.config_kF(0, 0.046, 0); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); + shooterWheelFalcon.configAllowableClosedloopError(0, 0, 0); // no "neutral" zone around target } @Override public void periodic() { // This method will be called once per scheduler run - UpdateDashboard(); updateHistory(); } @@ -261,13 +266,15 @@ public void setFeederSpeed(double pos) { * @param rpm */ public void setShooterWheelSpeed(double rpm) { - double ticks = convertRpmToTicksPer100ms(rpm); m_targetSpeedRPM = rpm; + double ticks = convertRpmToTicksPer100ms(rpm); shooterWheelLeft.set(ControlMode.Velocity, ticks); + shooterWheelRight.set(ControlMode.Velocity, ticks); } public void setShooterWheelSpeedVBus(double pos) { shooterWheelLeft.set(ControlMode.PercentOutput, pos); + shooterWheelRight.set(ControlMode.PercentOutput, pos); } public double getShooterWheelSpeed() { @@ -308,22 +315,25 @@ public double getF() { @Override public void setP(double d) { shooterWheelLeft.config_kP(0, d, 0); + shooterWheelRight.config_kP(0, d, 0); } @Override public void setI(double d) { shooterWheelLeft.config_kI(0, d, 0); + shooterWheelRight.config_kI(0, d, 0); } @Override public void setD(double d) { shooterWheelLeft.config_kD(0, d, 0); - + shooterWheelRight.config_kD(0, d, 0); } @Override public void setF(double d) { shooterWheelLeft.config_kF(0, d, 0); + shooterWheelRight.config_kF(0, d, 0); } } \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/MayhemDriverPad.java b/src/main/java/org/mayheminc/util/MayhemDriverPad.java index 999c3a7..766f0c5 100644 --- a/src/main/java/org/mayheminc/util/MayhemDriverPad.java +++ b/src/main/java/org/mayheminc/util/MayhemDriverPad.java @@ -81,38 +81,73 @@ public MayhemDriverPad() { } private static final double THROTTLE_DEAD_ZONE_PERCENT = 0.05; - private static final double STEERING_DEAD_ZONE_PERCENT = 0.05; + private static final double MIN_THROTTLE_FOR_MOVEMENT = 0.02; // what is the min throttle for Movement + private static final double NORMAL_MAX_THROTTLE = 1.00; // maximum speed is normally 100% + private static final double SLOW_MODE_MAX_THROTTLE = 0.30; // maximum throttle in "slow mode" is 30% public double driveThrottle() { // the driveThrottle is the "Y" axis of the Driver Gamepad. // However, the joysticks give -1.0 on the Y axis when pushed forward // This method reverses that, so that positive numbers are forward double throttleVal = -DRIVER_PAD.getY(); + double throttleAbs = Math.abs(throttleVal); + double maxPercentThrottle = NORMAL_MAX_THROTTLE; + + if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { + // check for "slow mode" and if so, constrain maxPercentThrottle to "SLOW_MODE_MAX_PERCENT" + maxPercentThrottle = SLOW_MODE_MAX_THROTTLE; + } + + // compute a scaled throttle magnitude, which will always be positive + double throttleMag = MIN_THROTTLE_FOR_MOVEMENT + (maxPercentThrottle - MIN_THROTTLE_FOR_MOVEMENT) * throttleAbs * throttleAbs; if (Math.abs(throttleVal) < THROTTLE_DEAD_ZONE_PERCENT) { + // check for throttle being in "dead zone" and if so, set throttle to zero throttleVal = 0.0; - } - - // if the slow button is pressed, cut the throttle value in third. - if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { - throttleVal = throttleVal / 3.0; + } else { + // make sure to preserve sign of throttle + if (throttleVal >= 0.0) { + throttleVal = throttleMag; + } else { + throttleVal = -throttleMag; + } } return (throttleVal); } + private static final double STEERING_DEAD_ZONE_PERCENT = 0.05; + private static final double MIN_STEERING_FOR_MOVEMENT = 0.02; + private static final double NORMAL_MAX_STEERING = 1.00; + private static final double SLOW_MODE_MAX_STEERING = 0.50; // maximum steering in "slow mode" is 50% + public double steeringX() { // SteeringX is the "X" axis of the right stick on the Driver Gamepad. - double value = DRIVER_PAD.getRawAxis(GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_X_AXIS); - if (Math.abs(value) < STEERING_DEAD_ZONE_PERCENT) { - value = 0.0; - } + double steeringVal = DRIVER_PAD.getRawAxis(GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_X_AXIS); + double steeringAbs = Math.abs(steeringVal); + double maxPercentSteering = NORMAL_MAX_STEERING; - // if the slow button is pressed, cut the steering value in half. if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { - value = value / 2.0; + // check for "slow mode" and if so, constrain maxPercentSteering to "SLOW_MODE_MAX_PERCENT" + maxPercentSteering = SLOW_MODE_MAX_STEERING; + } + + // compute a scaled steering magnitude, which will always be positive + double steeringMag = MIN_STEERING_FOR_MOVEMENT + (maxPercentSteering - MIN_STEERING_FOR_MOVEMENT) * steeringAbs; + + if (Math.abs(steeringVal) < STEERING_DEAD_ZONE_PERCENT) { + // check for steering being in "dead zone" and if so, set steering to zero + steeringVal = 0.0; + } else { + // make sure to preserve sign of steering + if (steeringVal >= 0.0) { + steeringVal = steeringMag; + } else { + steeringVal = -steeringMag; + } } - return value; + + return (steeringVal); } public boolean quickTurn() { diff --git a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java index 29b63fa..f834a0b 100644 --- a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java +++ b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -104,33 +104,10 @@ public void configPeakOutputVoltage(double d, double e) { } - public void setPID(double wheelP, double wheelI, double wheelD, double wheelF, int i, double m_voltageRampRate, - int j) { - this.config_kP(pidProfile, wheelP, 1000); - this.config_kI(pidProfile, wheelI, 1000); - this.config_kD(pidProfile, wheelD, 1000); - this.config_kF(pidProfile, wheelF, 1000); - } - public double getError() { return this.getClosedLoopError(0); } - int pidProfile; - - public void setProfile(int pidSlot) { - pidProfile = pidSlot; - } - - public void setPID(double pDown, double iDown, double dDown) { - setPID(pDown, iDown, dDown, 0.0, 0, 0.0, 0); - } - - public void setVoltageRampRate(double d) { - // Need to convert volts per second to time - this.configClosedloopRamp(0, 0); - } - public void setPosition(int zeroPositionCount) { this.setSelectedSensorPosition(zeroPositionCount, 0, 1000); } From 24047d6f697020048ffe68a4e5cff706fb9a79cf Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Mon, 24 Feb 2020 22:39:00 -0500 Subject: [PATCH 071/121] Air Compresor Pause added and IR sensor code --- .../commands/AirCompressorDefault.java | 11 ++++++- .../commands/AirCompressorPause.java | 29 +++++++++++++++++++ .../robot2020/subsystems/AirCompressor.java | 10 +++++++ .../robot2020/subsystems/Chimney.java | 15 ++++++++++ 4 files changed, 64 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/AirCompressorPause.java diff --git a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java index 690c53a..816e6d4 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java +++ b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java @@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj2.command.CommandBase; public class AirCompressorDefault extends CommandBase { + boolean value; + /** * Creates a new AirCompressorDefault. */ @@ -20,11 +22,18 @@ public AirCompressorDefault() { addRequirements(RobotContainer.compressor); } + public AirCompressorDefault(boolean b) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.compressor); + value = b; + + } + // Called when the command is initially scheduled. @Override public void initialize() { // robot.is - RobotContainer.compressor.setCompressor(true); + RobotContainer.compressor.setCompressor(value); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorPause.java b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorPause.java new file mode 100644 index 0000000..0cca5be --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorPause.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AirCompressorPause extends InstantCommand { + /** + * Creates a new AirCompressorSet. + */ + public AirCompressorPause() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.compressor); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.compressor.setCompressor(false); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java index fc1064e..7da847d 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java @@ -8,10 +8,12 @@ package org.mayheminc.robot2020.subsystems; import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class AirCompressor extends SubsystemBase { Compressor compressor = new Compressor(); + Timer m_Timer = new Timer(); /** * Creates a new compressor. @@ -21,17 +23,25 @@ public AirCompressor() { setCompressor(true); } + final double COPRESSOR_PAUSE_TIME = 10.0; + public void setCompressor(boolean b) { // b = false; if (b) { compressor.start(); } else { compressor.stop(); + m_Timer.start(); } } @Override public void periodic() { // This method will be called once per scheduler run + + // if the timer expires, turn on the compressor. + if (m_Timer.hasPeriodPassed(COPRESSOR_PAUSE_TIME)) { + setCompressor(true); + } } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java index 9205962..72028a1 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -12,12 +12,15 @@ import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.RangeFinder_GP2D120; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Chimney extends SubsystemBase { private final MayhemTalonSRX chimneyTalon = new MayhemTalonSRX(Constants.Talon.MAGAZINE_CHIMNEY); + private final RangeFinder_GP2D120 frontIR = new RangeFinder_GP2D120(2, 0); + private final RangeFinder_GP2D120 middleIR = new RangeFinder_GP2D120(3, 0); /** * Creates a new Chimney. @@ -34,11 +37,15 @@ public void periodic() { // This method will be called once per scheduler run updateSmartDashboard(); monitorTurntableMovement(); + frontIR.periodic(); + middleIR.periodic(); } void updateSmartDashboard() { SmartDashboard.putNumber("Chimney Speed", chimneyTalon.getSpeed()); SmartDashboard.putNumber("Chimney Current", chimneyTalon.getStatorCurrent()); + SmartDashboard.putBoolean("Chimney FrontIR", frontIR.isObjectClose()); + SmartDashboard.putBoolean("Chimney MidddleIR", middleIR.isObjectClose()); } void monitorTurntableMovement() { @@ -47,4 +54,12 @@ void monitorTurntableMovement() { public void setChimneySpeed(double speed) { chimneyTalon.set(ControlMode.PercentOutput, speed); } + + public boolean isBallInFrontOfChimney() { + return frontIR.isObjectClose(); + } + + public boolean isBallInMiddleOfChimney() { + return middleIR.isObjectClose(); + } } From 2d86cbe170cb95f400dfeae052270fb22caa1c1a Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Tue, 25 Feb 2020 00:27:51 -0500 Subject: [PATCH 072/121] Changed TalonSRX to VictorSPX for Chimney and Intake. --- .../mayheminc/robot2020/subsystems/Chimney.java | 17 ++++++++++++----- .../mayheminc/robot2020/subsystems/Intake.java | 14 ++++++++------ 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java index 9205962..a83b504 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -9,23 +9,30 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; +import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Chimney extends SubsystemBase { - private final MayhemTalonSRX chimneyTalon = new MayhemTalonSRX(Constants.Talon.MAGAZINE_CHIMNEY); + private final VictorSPX chimneyTalon = new VictorSPX(Constants.Talon.MAGAZINE_CHIMNEY); /** * Creates a new Chimney. */ public Chimney() { chimneyTalon.setNeutralMode(NeutralMode.Coast); - chimneyTalon.configNominalOutputVoltage(+0.0f, -0.0f); - chimneyTalon.configPeakOutputVoltage(+12.0, -12.0); + // chimneyTalon.configNominalOutputVoltage(+0.0f, -0.0f); + // chimneyTalon.configPeakOutputVoltage(+12.0, -12.0); + chimneyTalon.configNominalOutputForward(+0.0f); + chimneyTalon.configNominalOutputReverse(0.0); + chimneyTalon.configPeakOutputForward(+12.0); + chimneyTalon.configPeakOutputReverse(-12.0); + chimneyTalon.setInverted(true); } @@ -37,8 +44,8 @@ public void periodic() { } void updateSmartDashboard() { - SmartDashboard.putNumber("Chimney Speed", chimneyTalon.getSpeed()); - SmartDashboard.putNumber("Chimney Current", chimneyTalon.getStatorCurrent()); + SmartDashboard.putNumber("Chimney Speed", chimneyTalon.getMotorOutputPercent()); + // SmartDashboard.putNumber("Chimney Current", chimneyTalon.getStatorCurrent()); } void monitorTurntableMovement() { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index c328c64..ca58162 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -11,18 +11,20 @@ import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.PidTunerObject; +import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase implements PidTunerObject { private final int PIVOT_CLOSE_ENOUGH = 50; - private final MayhemTalonSRX rollerTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_ROLLERS); + private final VictorSPX rollerTalon = new VictorSPX(Constants.Talon.INTAKE_ROLLERS); private final MayhemTalonSRX pivotTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_PIVOT); private final int PIVOT_ZERO_POSITION = 900; public final double PIVOT_UP = 900.0; @@ -44,8 +46,10 @@ enum PivotMode { */ public Intake() { rollerTalon.setNeutralMode(NeutralMode.Coast); - rollerTalon.configNominalOutputVoltage(+0.0f, -0.0f); - rollerTalon.configPeakOutputVoltage(+12.0, -12.0); + rollerTalon.configNominalOutputForward(+0.0f); + rollerTalon.configNominalOutputReverse(0.0); + rollerTalon.configPeakOutputForward(+12.0); + rollerTalon.configPeakOutputReverse(-12.0); configPivotMotor(pivotTalon); } @@ -93,12 +97,10 @@ public void zero() { */ public void setRollers(double power) { rollerTalon.set(ControlMode.PercentOutput, power); - } public void setPivot(Double b) { m_targetPosition = b; - // pivotTalon.set(ControlMode.Position, b); mode = PivotMode.PID_MODE; isMoving = true; @@ -159,7 +161,7 @@ public void updateSmartDashBoard() { SmartDashboard.putBoolean("Intake Is Moving", isMoving); SmartDashboard.putBoolean("Intake PID Mode", (mode == PivotMode.PID_MODE)); - SmartDashboard.putNumber("Intake Rollers", rollerTalon.getMotorOutputVoltage()); + SmartDashboard.putNumber("Intake Rollers", rollerTalon.getMotorOutputPercent()); } public void setPivotVBus(double VBus) { From 1825564a515846b8eeeeebf3c25e52527fd93c3e Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Tue, 25 Feb 2020 02:54:00 -0500 Subject: [PATCH 073/121] Tweaked aim point a little bit to the right by setting to 0.510 (had been missing to the left slightly when at 0.500) Also added a "waitDuration" to ShooterReadyAimFire, and put some initial waitDurations into the "TrenchAuto." --- .../mayheminc/robot2020/autonomousroutines/TrenchAuto.java | 4 ++-- .../org/mayheminc/robot2020/commands/ShooterReadyAimFire.java | 4 ++-- .../java/org/mayheminc/robot2020/subsystems/Targeting.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java index bc74a6b..5d30c00 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java @@ -24,7 +24,7 @@ public TrenchAuto() { // first, shoot the balls that were pre-loaded addCommands(new IntakeSetPositionWithoutWaiting(RobotContainer.intake.PIVOT_DOWN)); - addCommands(new ShooterReadyAimFire()); + addCommands(new ShooterReadyAimFire(3.0)); // then, perform a 3-point turn addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 48, 160)); @@ -41,7 +41,7 @@ public TrenchAuto() { addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0)); addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 6, 0)); - addCommands(new ShooterReadyAimFire()); + addCommands(new ShooterReadyAimFire(6.0)); // turn the wheel off now that the shooting is all done addCommands( new ShooterSetWheel(0.0)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index 6259397..4878ed2 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -20,7 +20,7 @@ public class ShooterReadyAimFire extends SequentialCommandGroup { /** * Creates a new ShooterReadyAimFire. */ - public ShooterReadyAimFire() { + public ShooterReadyAimFire(double waitDuration) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); super(); @@ -43,7 +43,7 @@ public ShooterReadyAimFire() { new ShooterSetFeeder(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySetChimney(0.5)), new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)), - new Wait(6.0))); + new Wait(waitDuration))); addCommands(new ParallelRaceGroup( new MagazineSetTurntable(0.0), diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index d9a57be..d679551 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -31,7 +31,7 @@ public class Targeting extends SubsystemBase { private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 76.5; // Define the "target location" to be halfway from left to right - private final double CENTER_OF_TARGET_X = 0.5; + private final double CENTER_OF_TARGET_X = 0.510; // Calculate ticks per degree. // encoder ticks * turret pulley teeth / drive pulley teeth / 360 degrees From 29f9c9d9fd8912caf49169a3b70387b7c7eece1d Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Tue, 25 Feb 2020 19:41:04 -0500 Subject: [PATCH 074/121] added match number smartdashboard --- .../autonomousroutines/TrenchAuto.java | 4 +- .../commands/ShooterReadyAimFire.java | 37 ++++---- .../mayheminc/robot2020/subsystems/Drive.java | 93 ++++++++++--------- 3 files changed, 67 insertions(+), 67 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java index 5d30c00..4ba9b58 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java @@ -41,10 +41,8 @@ public TrenchAuto() { addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0)); addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 6, 0)); - addCommands(new ShooterReadyAimFire(6.0)); - // turn the wheel off now that the shooting is all done - addCommands( new ShooterSetWheel(0.0)); + addCommands(new ShooterSetWheel(0.0)); addCommands(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index 4878ed2..e52243e 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -20,35 +20,32 @@ public class ShooterReadyAimFire extends SequentialCommandGroup { /** * Creates a new ShooterReadyAimFire. */ - public ShooterReadyAimFire(double waitDuration) { + public ShooterReadyAimFire() { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); super(); addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); // addCommands(new ShooterSetWheel(3000.0, true)); - + // addCommands(new Wait(3.0)); - // TODO: Add compressor control so that compressor is turned off while actively shooting. - + // TODO: Add compressor control so that compressor is turned off while actively + // shooting. + addCommands(); + // aim to the target until we are on target. addCommands( - new ParallelRaceGroup( - new ParallelCommandGroup(new TargetingIsOnTarget(), new ShooterSetWheel(3000.0, true)), - new TurretAimToTarget())); - - // turn on the feeder, wiat 0.1, turn on the Chimney, wait 0.1, turn on the magazine, shoot for about 4 seconds - addCommands(new ParallelRaceGroup( - new ShooterSetFeeder(1.0), - new SequentialCommandGroup(new Wait(0.1), new ChimneySetChimney(0.5)), - new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)), - new Wait(waitDuration))); - - addCommands(new ParallelRaceGroup( - new MagazineSetTurntable(0.0), - new ChimneySetChimney(0.0), - new ShooterSetFeeder(0.0), - new Wait(0.1))); + new ParallelRaceGroup(new ParallelCommandGroup(new TargetingIsOnTarget(), new ShooterSetWheel(3000.0, true)), + new TurretAimToTarget())); + + // turn on the feeder, wiat 0.1, turn on the Chimney, wait 0.1, turn on the + // magazine, shoot for about 4 seconds + addCommands(new ParallelRaceGroup(new ShooterSetFeeder(1.0), + new SequentialCommandGroup(new Wait(0.1), new ChimneySetChimney(0.5)), + new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)), new Wait(6.0))); + + addCommands(new ParallelRaceGroup(new MagazineSetTurntable(0.0), new ChimneySetChimney(0.0), + new ShooterSetFeeder(0.0), new Wait(0.1))); } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 12c17cd..1f4ec3f 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.*; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.fasterxml.jackson.annotation.JacksonInject.Value; //import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -29,9 +30,9 @@ public class Drive extends SubsystemBase { public static final boolean COAST_MODE = false; // PID loop variables - private PIDController m_HeadingPid; - private PIDHeadingError m_HeadingError; - private PIDHeadingCorrection m_HeadingCorrection; + private final PIDController m_HeadingPid; + private final PIDHeadingError m_HeadingError; + private final PIDHeadingCorrection m_HeadingCorrection; private boolean m_HeadingPidPreventWindup = false; private static final int LOOPS_GYRO_DELAY = 10; @@ -45,21 +46,22 @@ public class Drive extends SubsystemBase { private AHRS Navx; // Driving mode - private boolean m_speedRacerDriveMode = true; // set by default + private final boolean m_speedRacerDriveMode = true; // set by default // NavX parameters private double m_desiredHeading = 0.0; private boolean m_useHeadingCorrection = true; - private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR + private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR private static final double kToleranceDegreesPIDControl = 0.2; // Drive parameters // pi * diameter * (pulley ratios) / (counts per rev * gearbox reduction) - public static final double DISTANCE_PER_PULSE_IN_INCHES = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected for 2020 + public static final double DISTANCE_PER_PULSE_IN_INCHES = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected + // for 2020 private boolean m_closedLoopMode = false; - private double m_maxWheelSpeed = 1.0; // should be maximum wheel speed in native units - private static final double CLOSED_LOOP_RAMP_RATE = 0.1; // time from neutral to full in seconds + private final double m_maxWheelSpeed = 1.0; // should be maximum wheel speed in native units + private static final double CLOSED_LOOP_RAMP_RATE = 0.1; // time from neutral to full in seconds private double m_initialWheelDistance = 0.0; private int m_iterationsSinceRotationCommanded = 0; @@ -84,7 +86,7 @@ public Drive() { */ Navx = new AHRS(SPI.Port.kMXP); Navx.reset(); - } catch (RuntimeException ex) { + } catch (final RuntimeException ex) { DriverStation.reportError("Error instantiating navX MXP: " + ex.getMessage(), true); System.out.println("Error loading navx."); } @@ -128,7 +130,7 @@ public void init() { configureDriveTalon(rightFrontTalon); } - public void zeroHeadingGyro(double headingOffset) { + public void zeroHeadingGyro(final double headingOffset) { Navx.zeroYaw(); setHeadingOffset(headingOffset); @@ -146,14 +148,14 @@ public void initDefaultCommand() { // setDefaultCommand(new SpeedRacerDrive()); } - private void configureDriveTalon(MayhemTalonSRX talon) { - double wheelP = 1.5; - double wheelI = 0.0; - double wheelD = 0.0; - double wheelF = 1.0; + private void configureDriveTalon(final MayhemTalonSRX talon) { + final double wheelP = 1.5; + final double wheelI = 0.0; + final double wheelD = 0.0; + final double wheelF = 1.0; talon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); - + talon.configNominalOutputVoltage(+0.0f, -0.0f); talon.configPeakOutputVoltage(+12.0, -12.0); @@ -161,7 +163,7 @@ private void configureDriveTalon(MayhemTalonSRX talon) { talon.config_kI(0, wheelI); talon.config_kD(0, wheelD); talon.config_kF(0, wheelF); - talon.configClosedloopRamp(CLOSED_LOOP_RAMP_RATE); // specify minimum time for neutral to full in seconds + talon.configClosedloopRamp(CLOSED_LOOP_RAMP_RATE); // specify minimum time for neutral to full in seconds DriverStation.reportError("setWheelPIDF: " + wheelP + " " + wheelI + " " + wheelD + " " + wheelF + "\n", false); } @@ -172,9 +174,9 @@ private void configureDriveTalon(MayhemTalonSRX talon) { * @param brakeMode - true for "brake in neutral" and false for "coast in * neutral" */ - public void setBrakeMode(boolean brakeMode) { + public void setBrakeMode(final boolean brakeMode) { - NeutralMode mode = (brakeMode) ? NeutralMode.Brake : NeutralMode.Coast; + final NeutralMode mode = (brakeMode) ? NeutralMode.Brake : NeutralMode.Coast; leftFrontTalon.setNeutralMode(mode); leftRearTalon.setNeutralMode(mode); @@ -221,8 +223,8 @@ public double getLeftSpeed() { // *************************** GYRO ******************************************* - public double calculateHeadingError(double Target) { - double currentHeading = getHeading(); + public double calculateHeadingError(final double Target) { + final double currentHeading = getHeading(); double error = Target - currentHeading; error = error % 360.0; if (error > 180.0) { @@ -235,7 +237,7 @@ public boolean getHeadingCorrectionMode() { return m_useHeadingCorrection; } - public void setHeadingCorrectionMode(boolean useHeadingCorrection) { + public void setHeadingCorrectionMode(final boolean useHeadingCorrection) { m_useHeadingCorrection = useHeadingCorrection; } @@ -244,8 +246,8 @@ private void resetAndEnableHeadingPID() { m_HeadingPid.setP(HEADING_PID_P); // } else // { - // low gear - // m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR); + // low gear + // m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR); // } m_HeadingPid.reset(); m_HeadingPid.enable(); @@ -256,11 +258,11 @@ private void resetAndEnableHeadingPID() { static private double m_prevRightDistance = 0.0; public boolean isStationary() { - double leftDistance = getLeftEncoder(); - double rightDistance = getRightEncoder(); + final double leftDistance = getLeftEncoder(); + final double rightDistance = getRightEncoder(); - double leftDelta = Math.abs(leftDistance - m_prevLeftDistance); - double rightDelta = Math.abs(rightDistance - m_prevRightDistance); + final double leftDelta = Math.abs(leftDistance - m_prevLeftDistance); + final double rightDelta = Math.abs(rightDistance - m_prevRightDistance); m_prevLeftDistance = leftDistance; m_prevRightDistance = rightDistance; @@ -279,7 +281,7 @@ public void displayGyroInfo() { private double m_headingOffset = 0.0; - public void setHeadingOffset(double arg_offset) { + public void setHeadingOffset(final double arg_offset) { m_headingOffset = arg_offset; } @@ -346,7 +348,7 @@ public void updateSdbPdp() { double rf; double lb; double rb; - double fudgeFactor = 0.0; + final double fudgeFactor = 0.0; lf = pdp.getCurrent(Constants.PDP.DRIVE_LEFT_A) - fudgeFactor; rf = pdp.getCurrent(Constants.PDP.DRIVE_LEFT_B) - fudgeFactor; @@ -357,6 +359,7 @@ public void updateSdbPdp() { SmartDashboard.putNumber("Right Front I", rf); SmartDashboard.putNumber("Left Back I", lb); SmartDashboard.putNumber("Right Back I", rb); + } /* @@ -394,12 +397,11 @@ public void setAutoAlignFalse() { // Robot.lights.set(LedPatternFactory.autoAlignGotIt); } - public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickTurn) { + public void speedRacerDrive(final double throttle, final double rawSteeringX, final boolean quickTurn) { speedRacerDriveNew(throttle, rawSteeringX, quickTurn); } - - public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean quickTurn) { + public void speedRacerDriveNew(final double throttle, final double rawSteeringX, final boolean quickTurn) { double leftPower, rightPower; double rotation = 0; final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast. @@ -411,7 +413,8 @@ public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean qui throttleSign = -1; } - // check for if steering input is essentially zero for "DriveStraight" functionality + // check for if steering input is essentially zero for "DriveStraight" + // functionality if ((-0.01 < rawSteeringX) && (rawSteeringX < 0.01)) { // no turn being commanded, drive straight or stay still m_iterationsSinceRotationCommanded++; @@ -476,11 +479,10 @@ public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean qui setMotorPower(leftPower, rightPower); } - - public void speedRacerDriveOld(double throttle, double rawSteeringX, boolean quickTurn) { + public void speedRacerDriveOld(final double throttle, final double rawSteeringX, final boolean quickTurn) { double leftPower, rightPower; double rotation = 0; - double adjustedSteeringX = rawSteeringX * throttle; + final double adjustedSteeringX = rawSteeringX * throttle; final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast. final double STD_TURN_GAIN = 0.9; // 2019: 1.5. 2020: 1.5 was too fast// the driver wants the non-quick turn // turning a little more responsive. @@ -611,7 +613,7 @@ private double maintainHeading() { return headingCorrection; } - public void rotate(double RotateX) { + public void rotate(final double RotateX) { m_desiredHeading += RotateX; if (m_desiredHeading > 180) { m_desiredHeading -= 360; @@ -623,7 +625,7 @@ public void rotate(double RotateX) { m_iterationsSinceMovementCommanded = 0; } - public void rotateToHeading(double desiredHeading) { + public void rotateToHeading(final double desiredHeading) { m_desiredHeading = desiredHeading; } @@ -643,6 +645,9 @@ public void updateSmartDashboard() { // ***** KBS: Uncommenting below, as it takes a LONG time to get PDP values // updateSdbPdp(); + int matchnumber = DriverStation.getInstance().getMatchNumber(); + DriverStation.MatchType MatchType = DriverStation.getInstance().getMatchType(); + SmartDashboard.putString("matchInfo", "" + MatchType + '_' + matchnumber); SmartDashboard.putNumber("Left Front Encoder Counts", leftFrontTalon.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Right Front Encoder Counts", rightFrontTalon.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Left Rear Encoder Counts", leftRearTalon.getSelectedSensorPosition(0)); @@ -687,13 +692,13 @@ public void updateSmartDashboard() { private static final double CAMERA_LAG = 0.150; // was .200; changing to .150 at CMP public void updateHistory() { - double now = Timer.getFPGATimestamp(); + final double now = Timer.getFPGATimestamp(); headingHistory.add(now, getHeading()); } public double getHeadingForCapturedImage() { - double now = Timer.getFPGATimestamp(); - double indexTime = now - CAMERA_LAG; + final double now = Timer.getFPGATimestamp(); + final double indexTime = now - CAMERA_LAG; return headingHistory.getAzForTime(indexTime); } @@ -712,12 +717,12 @@ public void saveInitialWheelDistance() { * @return */ public double getWheelDistance() { - double dist = (getLeftEncoder() + getRightEncoder()) / 2; + final double dist = (getLeftEncoder() + getRightEncoder()) / 2; return dist - m_initialWheelDistance; } // NOTE the difference between rotateToHeading(...) and goToHeading(...) - public void setDesiredHeading(double desiredHeading) { + public void setDesiredHeading(final double desiredHeading) { m_desiredHeading = desiredHeading; m_iterationsSinceRotationCommanded = LOOPS_GYRO_DELAY + 1; m_iterationsSinceMovementCommanded = 0; From fe5416db3b750e1559e4bf271e9973338d154b41 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Tue, 25 Feb 2020 22:41:21 -0500 Subject: [PATCH 075/121] Converted feed talon to victor spx. --- .../mayheminc/robot2020/subsystems/Shooter.java | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index c3545a9..b37f38c 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -5,6 +5,7 @@ import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -20,7 +21,7 @@ public class Shooter extends SubsystemBase implements PidTunerObject { private final MayhemTalonSRX shooterWheelRight = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_RIGHT); private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); - private final MayhemTalonSRX feederTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_FEEDER); + private final VictorSPX feederTalon = new VictorSPX(Constants.Talon.SHOOTER_FEEDER); // private final double MAX_SPEED_RPM = 5760.0; private final double TALON_TICKS_PER_REV = 2048.0; @@ -66,10 +67,13 @@ public void init() { } private void configureFeederTalon() { - feederTalon.changeControlMode(ControlMode.PercentOutput); + // feederTalon.changeControlMode(ControlMode.PercentOutput); feederTalon.setNeutralMode(NeutralMode.Brake); - feederTalon.configNominalOutputVoltage(+0.0f, -0.0f); - feederTalon.configPeakOutputVoltage(+6.0, -6.0); + + feederTalon.configNominalOutputForward(+0.0f); + feederTalon.configNominalOutputReverse(0.0); + feederTalon.configPeakOutputForward(+6.0); + feederTalon.configPeakOutputReverse(-6.0); } private void configureHoodTalon() { @@ -198,7 +202,7 @@ private void UpdateDashboard() { SmartDashboard.putNumber("Shooter turret velocity", turretTalon.getSelectedSensorVelocity(0)); SmartDashboard.putNumber("Shooter hood pos", hoodTalon.getPosition()); - SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getPosition()); + SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getMotorOutputVoltage()); // SmartDashboard.putNumber("Shooter Debug", debugShooter++); } From 468e7c1773d3af375cf0cf88e45917075d3d2820 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Wed, 26 Feb 2020 14:04:48 -0500 Subject: [PATCH 076/121] Massive refactoring of Shooter into four different subsystems: ShooterWheel, Feeder, Hood, Turret, ShooterWheel --- .../mayheminc/robot2020/RobotContainer.java | 38 +- .../autonomousroutines/DriveStraight.java | 29 -- .../ShootAndDriveForward.java | 4 +- .../autonomousroutines/StayStill.java | 2 +- .../autonomousroutines/TestTurret.java | 66 ++-- .../autonomousroutines/TrenchAuto.java | 30 +- .../robot2020/commands/DriveZeroGyro.java | 8 +- .../robot2020/commands/ShooterAdjustHood.java | 39 -- .../commands/ShooterAdjustWheel.java | 39 -- .../commands/ShooterAdjustWheelVBus.java | 33 -- .../commands/ShooterFireWhenReady.java | 6 +- .../commands/ShooterReadyAimFire.java | 25 +- .../robot2020/commands/ShooterSetFeeder.java | 40 -- .../robot2020/commands/ShooterSetHoodAbs.java | 33 -- .../robot2020/commands/ShooterSetHoodRel.java | 33 -- .../commands/ShooterSetHoodVBus.java | 48 --- .../commands/ShooterSetTurretAbs.java | 38 -- .../commands/ShooterSetTurretRel.java | 38 -- .../commands/ShooterSetTurretVBus.java | 48 --- .../robot2020/commands/ShooterSetWheel.java | 51 --- .../commands/ShooterSetWheelVBus.java | 32 -- .../robot2020/commands/ShooterZero.java | 28 -- .../commands/SystemZeroIncludingGyro.java | 9 +- .../commands/TargetingIsOnTarget.java | 2 +- .../robot2020/commands/TurretAimToTarget.java | 4 +- .../robot2020/subsystems/AirCompressor.java | 4 +- .../robot2020/subsystems/Autonomous.java | 5 + .../robot2020/subsystems/Chimney.java | 2 - .../robot2020/subsystems/Climber.java | 1 - .../mayheminc/robot2020/subsystems/Drive.java | 132 +++++-- .../robot2020/subsystems/Intake.java | 1 - .../robot2020/subsystems/Shooter.java | 343 ------------------ .../robot2020/subsystems/Targeting.java | 6 +- .../org/mayheminc/util/MayhemDriverPad.java | 2 +- 34 files changed, 206 insertions(+), 1013 deletions(-) delete mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustHood.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheelVBus.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodAbs.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodRel.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodVBus.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretAbs.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretRel.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretVBus.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheelVBus.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterZero.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index dbfbdca..9d3818d 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -33,7 +33,10 @@ public class RobotContainer { public static final Climber climber = new Climber(); public static final Magazine magazine = new Magazine(); - public static final Shooter shooter = new Shooter(); + public static final ShooterWheel shooterWheel = new ShooterWheel(); + public static final Hood hood = new Hood(); + public static final Turret turret = new Turret(); + public static final Feeder feeder = new Feeder(); public static final Drive drive = new Drive(); public static final Intake intake = new Intake(); public static final Autonomous autonomous = new Autonomous(); @@ -62,13 +65,13 @@ public RobotContainer() { pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN, - RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, shooter); + RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, drive); cameraLights.set(true); } public static void init() { - shooter.init(); + shooterWheel.init(); } private void configureDefaultCommands() { @@ -84,7 +87,7 @@ private void configureDefaultCommands() { // which wants all the battery power available) would turn the compressor off when the command starts // and off when the command ends.) Then again, maybe the "defaultCommand" is a good way to do this // and I just don't understand the style yet. - compressor.setDefaultCommand(new AirCompressorDefault()); + // compressor.setDefaultCommand(new AirCompressorDefault()); } private void configureAutonomousPrograms() { @@ -93,7 +96,9 @@ private void configureAutonomousPrograms() { // TODO: fix so that auto program is shown not just when changed (as shows old setting sometimes) // autonomousPrograms.push(/* 01 */ new StayStill()); + autonomousPrograms.push(/* 01 */ new DriveStraightOnly()); autonomousPrograms.push(/* 00 */ new TrenchAuto()); + // autonomousPrograms.push( new ShooterReadyAimFire()); // autonomousPrograms.push(new TestTurret()); @@ -155,8 +160,8 @@ private void configureDriverPadButtons() { // about -30 degrees // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new // ShooterSetTurretVBus(+0.2));// about +30 degrees - DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); - DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); + DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbs(Hood.HOOD_INITIATION_LINE_POSITION)); + DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbs(Hood.HOOD_TARGET_ZONE_POSITION)); // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new // ShooterSetTurretRel(-200.0)); @@ -173,13 +178,15 @@ private void configureDriverPadButtons() { // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); // Debug shooter pid velocity - DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterAdjustWheel(100.0)); - DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterAdjustWheel(-100.0)); - DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheelVBus(0.0)); - DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterSetWheel(3000)); + DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(100.0)); + DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-100.0)); + DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0)); + DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(3000)); + + DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1)); // TODO: above hard-coded constant (3000) should be a named constant from Shooter.java - DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterSetFeeder(1.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0)); } @@ -207,12 +214,11 @@ private void configureOperatorPadButtons() { // IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new ShooterSetTurretVBus(+0.2)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new ShooterAdjustHood(+1000.0)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new ShooterAdjustHood(-1000.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new TurretSetVBus(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new TurretSetVBus(+0.2)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new HoodAdjust(+1000.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new HoodAdjust(-1000.0)); - // TODO: Consider if below should use "variable power" for climber or just always full speed? OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(1.0)); OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-1.0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java deleted file mode 100644 index 8dfa465..0000000 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraight.java +++ /dev/null @@ -1,29 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.autonomousroutines; - -import org.mayheminc.robot2020.commands.*; -import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; -import org.mayheminc.robot2020.subsystems.Shooter; - -import edu.wpi.first.wpilibj2.command.*; - -public class DriveStraight extends SequentialCommandGroup { - /** - * Add your docs here. - */ - public DriveStraight() { - - addCommands(new DriveZeroGyro()); - // addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 100, 0)); - addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 50, 0)); - addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); - // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0)); - - } -} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java index 884f9be..e2a0f26 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java @@ -7,7 +7,7 @@ package org.mayheminc.robot2020.autonomousroutines; -import org.mayheminc.robot2020.commands.ShooterSetWheel; +import org.mayheminc.robot2020.commands.ShooterWheelSet; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -23,7 +23,7 @@ public ShootAndDriveForward() { // super(new FooCommand(), new BarCommand()); super(); - addCommands(new ShooterSetWheel(5500)); + addCommands(new ShooterWheelSet(5500)); // addCommands(new Wait(2.0)); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java index 99476f7..f3d5a35 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java @@ -17,7 +17,7 @@ public class StayStill extends SequentialCommandGroup { public StayStill() { // Perform needed initialization - addCommands(new DriveZeroGyro()); + addCommands(new DriveZeroGyro(0.0)); // ALL DONE! } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java index b7ae554..18d6afe 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java @@ -7,10 +7,6 @@ package org.mayheminc.robot2020.autonomousroutines; -import org.mayheminc.robot2020.RobotContainer; -import org.mayheminc.robot2020.commands.ShooterSetTurretAbs; -import org.mayheminc.robot2020.commands.Wait; - import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -25,37 +21,37 @@ public TestTurret() { // super(new FooCommand(), new BarCommand()); super(); - addRequirements(RobotContainer.shooter); - - addCommands(new ShooterSetTurretAbs(-10)); - addCommands(new Wait(3)); - addCommands(new ShooterSetTurretAbs(0)); - addCommands(new Wait(3)); - - addCommands(new ShooterSetTurretAbs(10)); - addCommands(new Wait(3)); - addCommands(new ShooterSetTurretAbs(0)); - addCommands(new Wait(3)); - - addCommands(new ShooterSetTurretAbs(-20)); - addCommands(new Wait(3)); - addCommands(new ShooterSetTurretAbs(0)); - addCommands(new Wait(3)); - - addCommands(new ShooterSetTurretAbs(20)); - addCommands(new Wait(3)); - addCommands(new ShooterSetTurretAbs(0)); - addCommands(new Wait(3)); - - addCommands(new ShooterSetTurretAbs(-45)); - addCommands(new Wait(3)); - addCommands(new ShooterSetTurretAbs(0)); - addCommands(new Wait(3)); - - addCommands(new ShooterSetTurretAbs(45)); - addCommands(new Wait(3)); - addCommands(new ShooterSetTurretAbs(0)); - addCommands(new Wait(3)); + // addRequirements(RobotContainer.shooter); + + // addCommands(new ShooterSetTurretAbs(-10)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(10)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(-20)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(20)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(-45)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(45)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java index 4ba9b58..520a321 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java @@ -10,7 +10,7 @@ import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; -import org.mayheminc.robot2020.subsystems.Shooter; +import org.mayheminc.robot2020.subsystems.Hood; import edu.wpi.first.wpilibj2.command.*; @@ -20,30 +20,36 @@ public class TrenchAuto extends SequentialCommandGroup { */ public TrenchAuto() { - addCommands(new DriveZeroGyro()); + addCommands(new DriveZeroGyro(180.0)); // first, shoot the balls that were pre-loaded addCommands(new IntakeSetPositionWithoutWaiting(RobotContainer.intake.PIVOT_DOWN)); - addCommands(new ShooterReadyAimFire(3.0)); + addCommands(new TurretSetAbs(+23000.0)); + addCommands(new ShooterWheelSet(3000.0, false)); + addCommands(new Wait(2.0)); + addCommands(new ShooterReadyAimFire(2.5)); - // then, perform a 3-point turn - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 48, 160)); + // then, drive down the trench, jogging left a little + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 36, 150)); // pick up balls while heading down the trench. addCommands(new ParallelRaceGroup( // intake while driving down the trench new IntakeSetRollers(-1.0), - new SequentialCommandGroup(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 132, 180), - new Wait(0.5), new DriveStraightOnHeading(-0.2, DistanceUnits.INCHES, 12, 180)))); + new SequentialCommandGroup(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 180, 180), + new Wait(0.5), new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 180)))); // after getting all three balls, go back to shooting position - addCommands(new DriveStraightOnHeading(-0.2, DistanceUnits.INCHES, 24, 30)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0)); - addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 6, 0)); + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 48, 180)); + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 96, 160)); + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 24, 180)); + + addCommands(new ShooterReadyAimFire(6.0)); // turn the wheel off now that the shooting is all done - addCommands(new ShooterSetWheel(0.0)); + addCommands(new ShooterWheelSet(0.0)); - addCommands(new ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); + // turn the wheel off now that the shooting is all done + addCommands(new HoodSetAbs(Hood.HOOD_TARGET_ZONE_POSITION)); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java index 53f6cc7..ff8900c 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java @@ -11,15 +11,19 @@ */ public class DriveZeroGyro extends RobotDisabledCommand { - public DriveZeroGyro() { + private double m_headingOffset = 0.0; + + public DriveZeroGyro(double headingOffset) { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); addRequirements(RobotContainer.drive); + + m_headingOffset = headingOffset; } // Called just before this Command runs the first time public void initialize() { - RobotContainer.drive.zeroHeadingGyro(0.0); + RobotContainer.drive.zeroHeadingGyro(m_headingOffset); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustHood.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustHood.java deleted file mode 100644 index 99d1bef..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustHood.java +++ /dev/null @@ -1,39 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ShooterAdjustHood extends CommandBase { - - double m_adjust; - - /** - * Creates a new ShooterAdjustHood. - */ - public ShooterAdjustHood(double adjust) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - - m_adjust = adjust; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setHoodPosition(RobotContainer.shooter.getHoodPosition() + m_adjust); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java deleted file mode 100644 index e0e1d20..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheel.java +++ /dev/null @@ -1,39 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ShooterAdjustWheel extends CommandBase { - - double m_adjust; - - /** - * Creates a new ShooterAdjustWheel. - */ - public ShooterAdjustWheel(double adjust) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - - m_adjust = adjust; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setShooterWheelSpeed(RobotContainer.shooter.getShooterWheelTargetSpeed() + m_adjust); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheelVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheelVBus.java deleted file mode 100644 index 72c19fd..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterAdjustWheelVBus.java +++ /dev/null @@ -1,33 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ShooterAdjustWheelVBus extends InstantCommand { - double m_adjust; - - /** - * Creates a new ShooterAdjustWheelVBus. - */ - public ShooterAdjustWheelVBus(double adjust) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - m_adjust = adjust; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - double currentSpeed = RobotContainer.shooter.getShooterWheelSpeedVBus(); - RobotContainer.shooter.setShooterWheelSpeedVBus(currentSpeed + m_adjust); - } - -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java index 25eaac7..961033c 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java @@ -18,7 +18,7 @@ public class ShooterFireWhenReady extends CommandBase { */ public ShooterFireWhenReady() { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); + // addRequirements(RobotContainer.shooter); } // Called when the command is initially scheduled. @@ -39,8 +39,8 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - RobotContainer.shooter.setFeederSpeed(0); - RobotContainer.shooter.setShooterWheelSpeed(0); + RobotContainer.feeder.setSpeed(0); + RobotContainer.shooterWheel.setSpeed(0); } // Returns true when the command should end. diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index e52243e..9f1c77e 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -7,8 +7,7 @@ package org.mayheminc.robot2020.commands; -import org.mayheminc.robot2020.subsystems.Shooter; - +import org.mayheminc.robot2020.subsystems.Hood; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -20,32 +19,28 @@ public class ShooterReadyAimFire extends SequentialCommandGroup { /** * Creates a new ShooterReadyAimFire. */ - public ShooterReadyAimFire() { + public ShooterReadyAimFire(double waitDuration) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); super(); - addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); - // addCommands(new ShooterSetWheel(3000.0, true)); - - // addCommands(new Wait(3.0)); + addCommands(new HoodSetAbs(Hood.HOOD_INITIATION_LINE_POSITION)); - // TODO: Add compressor control so that compressor is turned off while actively - // shooting. - addCommands(); + // Turn off compressor while actively shooting. + addCommands(new AirCompressorPause()); // aim to the target until we are on target. addCommands( - new ParallelRaceGroup(new ParallelCommandGroup(new TargetingIsOnTarget(), new ShooterSetWheel(3000.0, true)), + new ParallelRaceGroup(new ParallelCommandGroup(new TargetingIsOnTarget(), new ShooterWheelSet(3000.0, true)), new TurretAimToTarget())); - // turn on the feeder, wiat 0.1, turn on the Chimney, wait 0.1, turn on the + // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the // magazine, shoot for about 4 seconds - addCommands(new ParallelRaceGroup(new ShooterSetFeeder(1.0), + addCommands(new ParallelRaceGroup(new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySetChimney(0.5)), - new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)), new Wait(6.0))); + new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)), new Wait(waitDuration))); addCommands(new ParallelRaceGroup(new MagazineSetTurntable(0.0), new ChimneySetChimney(0.0), - new ShooterSetFeeder(0.0), new Wait(0.1))); + new FeederSet(0.0), new Wait(0.1))); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java deleted file mode 100644 index 5889aab..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetFeeder.java +++ /dev/null @@ -1,40 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ShooterSetFeeder extends CommandBase { - double m_speed; - - /** - * Creates a new ShooterSetFeeder. - */ - public ShooterSetFeeder(double speed) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - - m_speed = speed; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setFeederSpeed(m_speed); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - - RobotContainer.shooter.setFeederSpeed(0.0); - - } - -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodAbs.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodAbs.java deleted file mode 100644 index fa642b5..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodAbs.java +++ /dev/null @@ -1,33 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ShooterSetHoodAbs extends InstantCommand { - double m_set; - - /** - * Creates a new ShooterSetHoodAbs. - */ - public ShooterSetHoodAbs(double set) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - - m_set = set; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setHoodPosition(m_set); - } - -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodRel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodRel.java deleted file mode 100644 index 7980987..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodRel.java +++ /dev/null @@ -1,33 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ShooterSetHoodRel extends InstantCommand { - double m_adjust; - - /** - * Creates a new ShooterSetHood. - */ - public ShooterSetHoodRel(double adjust) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - - m_adjust = adjust; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - double pos = RobotContainer.shooter.getHoodPosition(); - RobotContainer.shooter.setHoodPosition(pos + m_adjust); - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodVBus.java deleted file mode 100644 index 5c566f0..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetHoodVBus.java +++ /dev/null @@ -1,48 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ShooterSetHoodVBus extends CommandBase { - double m_power; - - /** - * Creates a new ShooterSetHoodVBus. - */ - public ShooterSetHoodVBus(double power) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - m_power = power; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setHoodVBus(m_power); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - RobotContainer.shooter.setHoodVBus(0.0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretAbs.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretAbs.java deleted file mode 100644 index eda4f31..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretAbs.java +++ /dev/null @@ -1,38 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ShooterSetTurretAbs extends CommandBase { - double m_setPoint; - - /** - * Creates a new ShooterSetTurret. - */ - public ShooterSetTurretAbs(double setPoint) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - - m_setPoint = setPoint; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setTurretPositionAbs(m_setPoint); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretRel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretRel.java deleted file mode 100644 index ff1d925..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretRel.java +++ /dev/null @@ -1,38 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ShooterSetTurretRel extends CommandBase { - double m_setPoint; - - /** - * Creates a new ShooterSetTurretRel. - */ - public ShooterSetTurretRel(double setPoint) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - - m_setPoint = setPoint; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setTurretPositionRel(m_setPoint); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretVBus.java deleted file mode 100644 index 205df2c..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetTurretVBus.java +++ /dev/null @@ -1,48 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ShooterSetTurretVBus extends CommandBase { - double m_power; - - /** - * Creates a new ShooterSetTurretVBus. - */ - public ShooterSetTurretVBus(double power) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - m_power = power; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setTurretVBus(m_power); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - RobotContainer.shooter.setTurretVBus(0.0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java deleted file mode 100644 index 4899986..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheel.java +++ /dev/null @@ -1,51 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ShooterSetWheel extends CommandBase { - double m_rpm; - boolean m_waitForSpeed; - - public ShooterSetWheel(double rpm) { - this(rpm, false); - } - - /** - * Creates a new ShooterSetWheel. - */ - public ShooterSetWheel(double rpm, boolean wait) { - // Use addRequirements() here to declare subsystem dependencies. - // addRequirements(RobotContainer.shooter); - - m_rpm = rpm; - m_waitForSpeed = wait; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setShooterWheelSpeed(m_rpm); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if( m_waitForSpeed) - { - return (Math.abs( m_rpm - RobotContainer.shooter.getShooterWheelSpeed() ) < 100); - } - else - { - return true; - } - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheelVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheelVBus.java deleted file mode 100644 index 5f6c287..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterSetWheelVBus.java +++ /dev/null @@ -1,32 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ShooterSetWheelVBus extends InstantCommand { - - double m_speed; - /** - * Creates a new ShooterSetWheelVBus. - */ - public ShooterSetWheelVBus(double speed) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - - m_speed = speed; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.setShooterWheelSpeedVBus(m_speed); - } - -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterZero.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterZero.java deleted file mode 100644 index 88e4fe4..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterZero.java +++ /dev/null @@ -1,28 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class ShooterZero extends InstantCommand { - public ShooterZero() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.shooter.zero(); - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java b/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java index 77879c8..399e276 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java +++ b/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java @@ -7,9 +7,9 @@ package org.mayheminc.robot2020.commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -public class SystemZeroIncludingGyro extends SequentialCommandGroup { +public class SystemZeroIncludingGyro extends ParallelCommandGroup { /** * Creates a new SystemZeroIncludingGyro. */ @@ -18,8 +18,9 @@ public SystemZeroIncludingGyro() { addCommands(new IntakeZero()); addCommands(new ClimberZero()); - addCommands(new ShooterZero()); - addCommands(new DriveZeroGyro()); + addCommands(new HoodZero()); + addCommands(new TurretZero()); + addCommands(new DriveZeroGyro(0.0)); } @Override diff --git a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java index b8e3e37..dfd9b68 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java @@ -54,7 +54,7 @@ public boolean isFinished() { // return bearingGood && wheelsGood; double targetPos = RobotContainer.targeting.getDesiredAzimuth(); - double turretPos = RobotContainer.shooter.getTurretPosition(); + double turretPos = RobotContainer.turret.getPosition(); return ( Math.abs( targetPos - turretPos) < 50); } diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java index 850f632..b1a3814 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java @@ -17,7 +17,7 @@ public class TurretAimToTarget extends CommandBase { */ public TurretAimToTarget() { // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.shooter); + addRequirements(RobotContainer.turret); } // Called when the command is initially scheduled. @@ -29,7 +29,7 @@ public void initialize() { @Override public void execute() { double pos = RobotContainer.targeting.getDesiredAzimuth(); - RobotContainer.shooter.setTurretPositionAbs(pos); + RobotContainer.turret.setPositionAbs(pos); } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java index 7da847d..5dc519e 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java @@ -23,7 +23,7 @@ public AirCompressor() { setCompressor(true); } - final double COPRESSOR_PAUSE_TIME = 10.0; + final double COMPRESSOR_PAUSE_TIME = 10.0; public void setCompressor(boolean b) { // b = false; @@ -40,7 +40,7 @@ public void periodic() { // This method will be called once per scheduler run // if the timer expires, turn on the compressor. - if (m_Timer.hasPeriodPassed(COPRESSOR_PAUSE_TIME)) { + if (m_Timer.hasPeriodPassed(COMPRESSOR_PAUSE_TIME)) { setCompressor(true); } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java index dfafde3..bae47f1 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java @@ -55,6 +55,11 @@ public void adjustProgramNumber(final int delta) { private final int MAX_DELAY = 9; + @Override + public void periodic() { + updateSmartDashboard(); + } + public void adjustDelay(final int delta) { delay += delta; if (delay < 0) { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java index ed6f129..a21eac8 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -12,10 +12,8 @@ import com.ctre.phoenix.motorcontrol.can.VictorSPX; import org.mayheminc.robot2020.Constants; -import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.RangeFinder_GP2D120; -import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 21df5a4..55f8941 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -33,7 +33,6 @@ public class Climber extends SubsystemBase { */ public Climber() { - // TODO: Add "soft limit" to top of climber travel to avoid "overwinding" // TODO: Add position control to go all the way to the top, and nearly all the way in for climbing winchLeft.setNeutralMode(NeutralMode.Brake); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 1f4ec3f..3b05bc4 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.*; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; -import com.fasterxml.jackson.annotation.JacksonInject.Value; //import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -17,11 +16,12 @@ //import org.mayheminc.robot2020.Robot; //import org.mayheminc.robot2019.RobotMap; import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.PidTunerObject; import org.mayheminc.util.Utils; // TODO: Address all deprecated code masked by @SuppressWarnings("removal") annotations (not just in Drive.java) @SuppressWarnings("removal") -public class Drive extends SubsystemBase { +public class Drive extends SubsystemBase implements PidTunerObject { History headingHistory = new History(); @@ -51,7 +51,7 @@ public class Drive extends SubsystemBase { // NavX parameters private double m_desiredHeading = 0.0; private boolean m_useHeadingCorrection = true; - private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR + private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR; was 0.007 in early 2020 private static final double kToleranceDegreesPIDControl = 0.2; // Drive parameters @@ -59,8 +59,8 @@ public class Drive extends SubsystemBase { public static final double DISTANCE_PER_PULSE_IN_INCHES = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected // for 2020 - private boolean m_closedLoopMode = false; - private final double m_maxWheelSpeed = 1.0; // should be maximum wheel speed in native units + private boolean m_closedLoopMode = true; + private final double m_maxWheelSpeed = 18000.0; // should be maximum wheel speed in native units private static final double CLOSED_LOOP_RAMP_RATE = 0.1; // time from neutral to full in seconds private double m_initialWheelDistance = 0.0; @@ -104,10 +104,10 @@ public Drive() { m_HeadingPid.setAbsoluteTolerance(kToleranceDegreesPIDControl); // confirm all four drive talons are in coast mode - leftFrontTalon.setNeutralMode(NeutralMode.Brake); - leftRearTalon.setNeutralMode(NeutralMode.Brake); - rightFrontTalon.setNeutralMode(NeutralMode.Brake); - rightRearTalon.setNeutralMode(NeutralMode.Brake); + leftFrontTalon.setNeutralMode(NeutralMode.Coast); + leftRearTalon.setNeutralMode(NeutralMode.Coast); + rightFrontTalon.setNeutralMode(NeutralMode.Coast); + rightRearTalon.setNeutralMode(NeutralMode.Coast); // set rear talons to follow their respective front talons leftRearTalon.follow(leftFrontTalon); @@ -119,6 +119,10 @@ public Drive() { leftRearTalon.setInverted(false); rightFrontTalon.setInverted(true); rightRearTalon.setInverted(true); + + // talon closed loop config + configureDriveTalon(leftFrontTalon); + configureDriveTalon(rightFrontTalon); } public void init() { @@ -149,20 +153,20 @@ public void initDefaultCommand() { } private void configureDriveTalon(final MayhemTalonSRX talon) { - final double wheelP = 1.5; - final double wheelI = 0.0; - final double wheelD = 0.0; - final double wheelF = 1.0; + final double wheelP = 0.020; + final double wheelI = 0.000; + final double wheelD = 0.200; + final double wheelF = 0.060; talon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); talon.configNominalOutputVoltage(+0.0f, -0.0f); talon.configPeakOutputVoltage(+12.0, -12.0); - talon.config_kP(0, wheelP); - talon.config_kI(0, wheelI); - talon.config_kD(0, wheelD); - talon.config_kF(0, wheelF); + talon.config_kP(0, wheelP, 0); + talon.config_kI(0, wheelI, 0); + talon.config_kD(0, wheelD, 0); + talon.config_kF(0, wheelF, 0); talon.configClosedloopRamp(CLOSED_LOOP_RAMP_RATE); // specify minimum time for neutral to full in seconds DriverStation.reportError("setWheelPIDF: " + wheelP + " " + wheelI + " " + wheelD + " " + wheelF + "\n", false); @@ -397,11 +401,11 @@ public void setAutoAlignFalse() { // Robot.lights.set(LedPatternFactory.autoAlignGotIt); } - public void speedRacerDrive(final double throttle, final double rawSteeringX, final boolean quickTurn) { + public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickTurn) { speedRacerDriveNew(throttle, rawSteeringX, quickTurn); } - public void speedRacerDriveNew(final double throttle, final double rawSteeringX, final boolean quickTurn) { + public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean quickTurn) { double leftPower, rightPower; double rotation = 0; final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast. @@ -450,30 +454,32 @@ public void speedRacerDriveNew(final double throttle, final double rawSteeringX, } m_iterationsSinceMovementCommanded = 0; } + // driveStraight code benefits from "spin" behavior when needed + leftPower = throttle + rotation; + rightPower = throttle - rotation; } else { // commanding a turn, reset iterationsSinceRotationCommanded m_iterationsSinceRotationCommanded = 0; m_iterationsSinceMovementCommanded = 0; - } - - if (quickTurn) { - // want a high-rate turn (also allows "spin" behavior) - // power to each wheel is a combination of the throttle and rotation - rotation = rawSteeringX * throttleSign * QUICK_TURN_GAIN; - leftPower = throttle + rotation; - rightPower = throttle - rotation; - } else { - // want a standard rate turn (scaled by the throttle) - if (rawSteeringX >= 0.0) { - // turning to the right, derate the right power by turn amount - // note that rawSteeringX is positive in this portion of the "if" - leftPower = throttle; - rightPower = throttle * (1.0 - Math.abs(rawSteeringX)); + if (quickTurn) { + // want a high-rate turn (also allows "spin" behavior) + // power to each wheel is a combination of the throttle and rotation + rotation = rawSteeringX * throttleSign * QUICK_TURN_GAIN; + leftPower = throttle + rotation; + rightPower = throttle - rotation; } else { - // turning to the left, derate the left power by turn amount - // note that rawSteeringX is negative in this portion of the "if" - leftPower = throttle * (1.0 - Math.abs(rawSteeringX)); - rightPower = throttle; + // want a standard rate turn (scaled by the throttle) + if (rawSteeringX >= 0.0) { + // turning to the right, derate the right power by turn amount + // note that rawSteeringX is positive in this portion of the "if" + leftPower = throttle; + rightPower = throttle * (1.0 - Math.abs(rawSteeringX)); + } else { + // turning to the left, derate the left power by turn amount + // note that rawSteeringX is negative in this portion of the "if" + leftPower = throttle * (1.0 - Math.abs(rawSteeringX)); + rightPower = throttle; + } } } setMotorPower(leftPower, rightPower); @@ -731,4 +737,54 @@ public void setDesiredHeading(final double desiredHeading) { resetAndEnableHeadingPID(); } + + + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return leftFrontTalon.getP(); + } + + @Override + public double getI() { + return leftFrontTalon.getI(); + } + + @Override + public double getD() { + return leftFrontTalon.getD(); + } + + @Override + public double getF() { + return leftFrontTalon.getF(); + + } + + @Override + public void setP(double d) { + leftFrontTalon.config_kP(0, d, 0); + rightFrontTalon.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + leftFrontTalon.config_kI(0, d, 0); + rightFrontTalon.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + leftFrontTalon.config_kD(0, d, 0); + rightFrontTalon.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + leftFrontTalon.config_kF(0, d, 0); + rightFrontTalon.config_kF(0, d, 0); + } + + } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index ca58162..5d30301 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -17,7 +17,6 @@ import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.PidTunerObject; -import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java index b37f38c..e69de29 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java @@ -1,343 +0,0 @@ -package org.mayheminc.robot2020.subsystems; - -import org.mayheminc.robot2020.Constants; -// import org.mayheminc.robot2020.RobotContainer; - -import com.ctre.phoenix.motorcontrol.*; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import org.mayheminc.util.History; -import org.mayheminc.util.MayhemTalonSRX; -// import org.mayheminc.util.PidTuner; -import org.mayheminc.util.PidTunerObject; - -public class Shooter extends SubsystemBase implements PidTunerObject { - private final MayhemTalonSRX shooterWheelLeft = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_LEFT); - private final MayhemTalonSRX shooterWheelRight = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_RIGHT); - private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); - private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); - private final VictorSPX feederTalon = new VictorSPX(Constants.Talon.SHOOTER_FEEDER); - - // private final double MAX_SPEED_RPM = 5760.0; - private final double TALON_TICKS_PER_REV = 2048.0; - private final double TURRET_MIN_POS = -5500.0; - private final double TURRET_MAX_POS = +5500.0; - private final double SECONDS_PER_MINUTE = 60.0; - private final double HUNDRED_MS_PER_SECOND = 10.0; - - public final static double HOOD_TARGET_ZONE_POSITION = 5000; - public final static double HOOD_INITIATION_LINE_POSITION = 65000; - public final static double HOOD_TRENCH_MID_POSITION = 80000; - - double m_targetSpeedRPM; - History headingHistory = new History(); - - // Note: for ease of thinking, 1 RPM =~ 3.4 native units for the shooter - double convertRpmToTicksPer100ms(double rpm) { - return rpm / SECONDS_PER_MINUTE * TALON_TICKS_PER_REV / HUNDRED_MS_PER_SECOND; - } - - // Note: 3.413 native units =~ 1.0 RPM for the shooter - double convertTicksPer100msToRPM(double ticks) { - return ticks * HUNDRED_MS_PER_SECOND / TALON_TICKS_PER_REV * SECONDS_PER_MINUTE; - } - - /** - * Creates a new Shooter. - */ - public Shooter() { - configureWheelFalcons(); - configureTurretTalon(); - configureHoodTalon(); - configureFeederTalon(); - } - - public void init() { - configureWheelFalcons(); - configureTurretTalon(); - configureHoodTalon(); - configureFeederTalon(); - - setShooterWheelSpeedVBus(0.0); - } - - private void configureFeederTalon() { - // feederTalon.changeControlMode(ControlMode.PercentOutput); - feederTalon.setNeutralMode(NeutralMode.Brake); - - feederTalon.configNominalOutputForward(+0.0f); - feederTalon.configNominalOutputReverse(0.0); - feederTalon.configPeakOutputForward(+6.0); - feederTalon.configPeakOutputReverse(-6.0); - } - - private void configureHoodTalon() { - hoodTalon.config_kP(0, 1.0, 0); - hoodTalon.config_kI(0, 0.0, 0); - hoodTalon.config_kD(0, 0.0, 0); - hoodTalon.config_kF(0, 0.0, 0); - - hoodTalon.changeControlMode(ControlMode.Position); - feederTalon.setNeutralMode(NeutralMode.Coast); - hoodTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - - hoodTalon.configNominalOutputVoltage(+0.0f, -0.0f); - hoodTalon.configPeakOutputVoltage(+12.0, -12.0); - hoodTalon.setInverted(true); - hoodTalon.setSensorPhase(true); - - hoodTalon.configForwardSoftLimitThreshold(100000); - hoodTalon.configForwardSoftLimitEnable(true); - hoodTalon.configReverseSoftLimitThreshold(0); - hoodTalon.configReverseSoftLimitEnable(true); - } - - void configureTurretTalon() { - turretTalon.config_kP(0, 1.0, 0); - turretTalon.config_kI(0, 0.0, 0); - turretTalon.config_kD(0, 0.0, 0); - turretTalon.config_kF(0, 0.0, 0); - turretTalon.changeControlMode(ControlMode.Position); - turretTalon.setNeutralMode(NeutralMode.Coast); - turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - - turretTalon.configMotionCruiseVelocity(800); // measured velocity of ~100K - // at 80%; set cruise to that - turretTalon.configMotionAcceleration(3200); // acceleration of 4x velocity - // allows cruise in 1/4 second - - turretTalon.configNominalOutputVoltage(+0.0f, -0.0f); - turretTalon.configPeakOutputVoltage(+4.0, -4.0); - - turretTalon.configForwardSoftLimitThreshold(6000); - turretTalon.configForwardSoftLimitEnable(true); - turretTalon.configReverseSoftLimitThreshold(-6000); - turretTalon.configReverseSoftLimitEnable(true); - - this.setTurretVBus(0.0); - } - - // configure a pair of shooter wheel falcons - private void configureWheelFalcons() { - // most of the configuration is shared for the two Falcons - configureOneWheelFalcon(shooterWheelLeft); - configureOneWheelFalcon(shooterWheelRight); - - // with the exception of one rotating the opposite direction - shooterWheelLeft.setInverted(false); - shooterWheelRight.setInverted(true); - } - private void configureOneWheelFalcon(MayhemTalonSRX shooterWheelFalcon) { - shooterWheelFalcon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); - shooterWheelFalcon.setNeutralMode(NeutralMode.Coast); - shooterWheelFalcon.configNominalOutputVoltage(+0.0f, -0.0f); - shooterWheelFalcon.configPeakOutputVoltage(+12.0, 0.0); - shooterWheelFalcon.configNeutralDeadband(0.001); // Config neutral deadband to be the smallest possible - - // For PIDF computations, 1023 is interpreted as "full" motor output - // Velocity is in native units of TicksPer100ms. - // 1000rpm =~ 3413 native units. - // P of "3.0" means that full power applied with error of 341 native units = 100rpm - // (above also means that 50% power boost applied with error of 50rpm) - shooterWheelFalcon.config_kP(0, 0.1, 0); // previously used 3.0 - shooterWheelFalcon.config_kI(0, 0.0, 0); - shooterWheelFalcon.config_kD(0, 0.0, 0); // CTRE recommends starting at 10x P-gain - shooterWheelFalcon.config_kF(0, 0.046, 0); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); - shooterWheelFalcon.configAllowableClosedloopError(0, 0, 0); // no "neutral" zone around target - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - UpdateDashboard(); - updateHistory(); - } - - // KBS: tuned below at practice field on 21 Feb 2020 via successive refinement. - private static final double CAMERA_LAG = 0.08; // .05 was best so far in 2020; used .150 in 2019 - - private void updateHistory() { - double now = Timer.getFPGATimestamp(); - headingHistory.add(now, getTurretPosition()); - } - - public double getAzimuthForCapturedImage() { - double now = Timer.getFPGATimestamp(); - double indexTime = now - CAMERA_LAG; - return headingHistory.getAzForTime(indexTime); - } - - private void UpdateDashboard() { - SmartDashboard.putNumber("Shooter Wheel pos", shooterWheelLeft.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelLeft.getSelectedSensorVelocity(0)); - SmartDashboard.putNumber("Shooter Wheel RPM", - convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0))); - - SmartDashboard.putNumber("Shooter Wheel target RPM", m_targetSpeedRPM); - SmartDashboard.putNumber("Shooter Wheel Error", - m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0))); - SmartDashboard.putNumber("Shooter Wheel Voltage", shooterWheelLeft.getMotorOutputVoltage()); - SmartDashboard.putNumber("Shooter Wheel Bus Voltage", shooterWheelLeft.getBusVoltage()); - SmartDashboard.putNumber("Shooter Wheel Current", shooterWheelLeft.getSupplyCurrent()); - - SmartDashboard.putNumber("Shooter Wheel R-pos", shooterWheelRight.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Shooter Wheel R-speed", shooterWheelRight.getSelectedSensorVelocity(0)); - SmartDashboard.putNumber("Shooter Wheel R-RPM", - convertTicksPer100msToRPM(shooterWheelRight.getSelectedSensorVelocity(0))); - - SmartDashboard.putNumber("Shooter Wheel R-target RPM", m_targetSpeedRPM); - SmartDashboard.putNumber("Shooter Wheel R-Error", - m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelRight.getSelectedSensorVelocity(0))); - SmartDashboard.putNumber("Shooter Wheel R-Voltage", shooterWheelRight.getMotorOutputVoltage()); - SmartDashboard.putNumber("Shooter Wheel R-Bus Voltage", shooterWheelRight.getBusVoltage()); - SmartDashboard.putNumber("Shooter Wheel R-Current", shooterWheelRight.getSupplyCurrent()); - - SmartDashboard.putNumber("Shooter turret pos", turretTalon.getPosition()); - SmartDashboard.putNumber("Shooter turret vbus", turretTalon.getMotorOutputVoltage()); - SmartDashboard.putNumber("Shooter turret velocity", turretTalon.getSelectedSensorVelocity(0)); - - SmartDashboard.putNumber("Shooter hood pos", hoodTalon.getPosition()); - SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getMotorOutputVoltage()); - - // SmartDashboard.putNumber("Shooter Debug", debugShooter++); - } - - public void zero() { - turretTalon.setPosition(0); - hoodTalon.setPosition(0); - } - - /** - * Set the absolute turret position. - */ - public void setTurretPositionAbs(double pos) { - if (pos < TURRET_MIN_POS) { - pos = TURRET_MIN_POS; - } - if (pos > TURRET_MAX_POS) { - pos = TURRET_MAX_POS; - } - // turretTalon.set(ControlMode.Position, pos); - turretTalon.set(ControlMode.MotionMagic, pos); - } - - /** - * Set the relative turret position - * - * @param pos number of encoder ticks to adjust. - */ - public void setTurretPositionRel(double pos) { - setTurretPositionAbs(getTurretPosition() + pos); - } - - public void setTurretVBus(double power) { - turretTalon.set(ControlMode.PercentOutput, power); - } - - /** - * Get the current position of the turret. - * - * @return - */ - public double getTurretPosition() { - return turretTalon.getPosition(); - } - - public void setHoodPosition(double pos) { - hoodTalon.set(ControlMode.Position, pos); - } - - public double getHoodPosition() { - return hoodTalon.getPosition(); - } - - public void setHoodVBus(double d) { - hoodTalon.set(ControlMode.PercentOutput, d); - } - - public void setFeederSpeed(double pos) { - feederTalon.set(ControlMode.PercentOutput, pos); - } - - /** - * Set shooter to rpm speed. - * - * @param rpm - */ - public void setShooterWheelSpeed(double rpm) { - m_targetSpeedRPM = rpm; - double ticks = convertRpmToTicksPer100ms(rpm); - shooterWheelLeft.set(ControlMode.Velocity, ticks); - shooterWheelRight.set(ControlMode.Velocity, ticks); - } - - public void setShooterWheelSpeedVBus(double pos) { - shooterWheelLeft.set(ControlMode.PercentOutput, pos); - shooterWheelRight.set(ControlMode.PercentOutput, pos); - } - - public double getShooterWheelSpeed() { - return convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0)); - } - - public double getShooterWheelTargetSpeed() { - return m_targetSpeedRPM; - } - - public double getShooterWheelSpeedVBus() { - return shooterWheelLeft.getMotorOutputVoltage(); - } - - //////////////////////////////////////////////////// - // PidTunerObject - @Override - public double getP() { - return shooterWheelLeft.getP(); - } - - @Override - public double getI() { - return shooterWheelLeft.getI(); - } - - @Override - public double getD() { - return shooterWheelLeft.getD(); - } - - @Override - public double getF() { - return shooterWheelLeft.getF(); - - } - - @Override - public void setP(double d) { - shooterWheelLeft.config_kP(0, d, 0); - shooterWheelRight.config_kP(0, d, 0); - } - - @Override - public void setI(double d) { - shooterWheelLeft.config_kI(0, d, 0); - shooterWheelRight.config_kI(0, d, 0); - } - - @Override - public void setD(double d) { - shooterWheelLeft.config_kD(0, d, 0); - shooterWheelRight.config_kD(0, d, 0); - } - - @Override - public void setF(double d) { - shooterWheelLeft.config_kF(0, d, 0); - shooterWheelRight.config_kF(0, d, 0); - } - -} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index d679551..ea90715 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -108,14 +108,14 @@ public void update() { m_bestY = 0.0; m_tilt = 0.0; m_area = 0.0; - m_desiredAzimuth = RobotContainer.shooter.getAzimuthForCapturedImage(); + m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); } else if (m_target_array[0] < 0.0) { // this means the array has no valid data. Set m_xError = 0.0 m_bestX = 0.0; m_bestY = 0.0; m_tilt = 0.0; m_area = 0.0; - m_desiredAzimuth = RobotContainer.shooter.getAzimuthForCapturedImage(); + m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); } else { // We have a valid data array. // There are three different situations: @@ -192,7 +192,7 @@ public double findDesiredAzimuth(double X, double Y, double tilt, double area) { ticksError = angleError * TICKS_PER_DEGREE; // Convert angleError into a desired azimuth, using the azimuth history - desiredAzimuth = ticksError + RobotContainer.shooter.getAzimuthForCapturedImage(); + desiredAzimuth = ticksError + RobotContainer.turret.getAzimuthForCapturedImage(); // Update SmartDashboard SmartDashboard.putNumber("Vision Angle Error", angleError); SmartDashboard.putNumber("Vision Desired Azimuth", desiredAzimuth); diff --git a/src/main/java/org/mayheminc/util/MayhemDriverPad.java b/src/main/java/org/mayheminc/util/MayhemDriverPad.java index 766f0c5..b4c4e5e 100644 --- a/src/main/java/org/mayheminc/util/MayhemDriverPad.java +++ b/src/main/java/org/mayheminc/util/MayhemDriverPad.java @@ -119,7 +119,7 @@ public double driveThrottle() { private static final double STEERING_DEAD_ZONE_PERCENT = 0.05; private static final double MIN_STEERING_FOR_MOVEMENT = 0.02; private static final double NORMAL_MAX_STEERING = 1.00; - private static final double SLOW_MODE_MAX_STEERING = 0.50; // maximum steering in "slow mode" is 50% + private static final double SLOW_MODE_MAX_STEERING = 0.80; // maximum steering in "slow mode" is 50% public double steeringX() { // SteeringX is the "X" axis of the right stick on the Driver Gamepad. From e3da220a375721c411ecf968649b062e7de52ce0 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Wed, 26 Feb 2020 14:10:07 -0500 Subject: [PATCH 077/121] Also adding the newly-named files as part of the Shooter subsystem breakup. --- .../autonomousroutines/DriveStraightOnly.java | 31 +++ .../robot2020/commands/DriveStraight.java | 52 +++++ .../robot2020/commands/FeederSet.java | 40 ++++ .../robot2020/commands/HoodAdjust.java | 39 ++++ .../robot2020/commands/HoodSetAbs.java | 33 ++++ .../robot2020/commands/HoodSetRel.java | 33 ++++ .../robot2020/commands/HoodSetVBus.java | 48 +++++ .../robot2020/commands/HoodZero.java | 28 +++ .../PrintAutonomousTimeRemaining.java | 27 +++ .../commands/ShooterFireWhenReady.java | 3 +- .../commands/ShooterWheelAdjust.java | 39 ++++ .../commands/ShooterWheelAdjustVBus.java | 33 ++++ .../robot2020/commands/ShooterWheelSet.java | 51 +++++ .../commands/ShooterWheelSetVBus.java | 32 +++ .../robot2020/commands/TurretSetAbs.java | 38 ++++ .../robot2020/commands/TurretSetRel.java | 38 ++++ .../robot2020/commands/TurretSetVBus.java | 48 +++++ .../robot2020/commands/TurretZero.java | 28 +++ .../robot2020/subsystems/Feeder.java | 47 +++++ .../mayheminc/robot2020/subsystems/Hood.java | 122 ++++++++++++ .../robot2020/subsystems/ShooterWheel.java | 187 ++++++++++++++++++ .../robot2020/subsystems/Turret.java | 172 ++++++++++++++++ 22 files changed, 1168 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraightOnly.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/DriveStraight.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/FeederSet.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/HoodAdjust.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/HoodSetRel.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/HoodSetVBus.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/HoodZero.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjust.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjustVBus.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetVBus.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/TurretSetRel.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/TurretSetVBus.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/TurretZero.java create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Feeder.java create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Hood.java create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Turret.java diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraightOnly.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraightOnly.java new file mode 100644 index 0000000..e44ce7d --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraightOnly.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; + +import edu.wpi.first.wpilibj2.command.*; + +public class DriveStraightOnly extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public DriveStraightOnly() { + + addCommands(new DriveZeroGyro(0.0)); + + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 50, 0)); + + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 270)); + + // addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); + // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0)); + + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/commands/DriveStraight.java new file mode 100644 index 0000000..4a9c784 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveStraight.java @@ -0,0 +1,52 @@ +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +// import org.mayheminc.robot2019.Robot; +// import org.mayheminc.robot2019.subsystems.Drive; + +// import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class DriveStraight extends CommandBase { + + double m_targetPower; + + /** + * + * @param arg_targetPower +/- motor power [-1.0, +1.0] + * @param arg_distance Distance in encoder counts + */ + public DriveStraight(double arg_targetSpeed) { + addRequirements(RobotContainer.drive); + + m_targetPower = arg_targetSpeed; + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + public void execute() { + RobotContainer.drive.speedRacerDrive(m_targetPower, 0, false); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return (false); + } + + // Called once after isFinished returns true + @Override + public void end(boolean interrupted) { + RobotContainer.drive.stop(); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/FeederSet.java b/src/main/java/org/mayheminc/robot2020/commands/FeederSet.java new file mode 100644 index 0000000..401b1c5 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/FeederSet.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class FeederSet extends CommandBase { + double m_speed; + + /** + * Creates a new ShooterSetFeeder. + */ + public FeederSet(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.feeder); + + m_speed = speed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.feeder.setSpeed(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + RobotContainer.feeder.setSpeed(0.0); + + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodAdjust.java b/src/main/java/org/mayheminc/robot2020/commands/HoodAdjust.java new file mode 100644 index 0000000..e84b602 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodAdjust.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class HoodAdjust extends CommandBase { + + double m_adjust; + + /** + * Creates a new ShooterAdjustHood. + */ + public HoodAdjust(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.setPosition(RobotContainer.hood.getPosition() + m_adjust); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java new file mode 100644 index 0000000..9b142a2 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class HoodSetAbs extends InstantCommand { + double m_set; + + /** + * Creates a new HoodSetAbs. + */ + public HoodSetAbs(double set) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + + m_set = set; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.setPosition(m_set); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetRel.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetRel.java new file mode 100644 index 0000000..404e57a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetRel.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class HoodSetRel extends InstantCommand { + double m_adjust; + + /** + * Creates a new ShooterSetHood. + */ + public HoodSetRel(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double pos = RobotContainer.hood.getPosition(); + RobotContainer.hood.setPosition(pos + m_adjust); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetVBus.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetVBus.java new file mode 100644 index 0000000..4f12d42 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetVBus.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class HoodSetVBus extends CommandBase { + double m_power; + + /** + * Creates a new ShooterSetHoodVBus. + */ + public HoodSetVBus(double power) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + m_power = power; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.setVBus(m_power); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.hood.setVBus(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodZero.java b/src/main/java/org/mayheminc/robot2020/commands/HoodZero.java new file mode 100644 index 0000000..d5548ad --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodZero.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class HoodZero extends InstantCommand { + public HoodZero() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.zero(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java new file mode 100644 index 0000000..ed73227 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java @@ -0,0 +1,27 @@ +package org.mayheminc.robot2020.commands; + +// import org.mayheminc.robot2020.Robot; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class PrintAutonomousTimeRemaining extends CommandBase { + String Message = ""; + public PrintAutonomousTimeRemaining(String msg) { + this.Message = msg; + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + // DriverStation.reportError(Message + " At: " + Robot.autonomousTimeRemaining() + "\n", false); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java index 961033c..1196e59 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java @@ -18,7 +18,8 @@ public class ShooterFireWhenReady extends CommandBase { */ public ShooterFireWhenReady() { // Use addRequirements() here to declare subsystem dependencies. - // addRequirements(RobotContainer.shooter); + addRequirements(RobotContainer.shooterWheel); + addRequirements(RobotContainer.feeder); } // Called when the command is initially scheduled. diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjust.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjust.java new file mode 100644 index 0000000..5db4626 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjust.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterWheelAdjust extends CommandBase { + + double m_adjust; + + /** + * Creates a new ShooterAdjustWheel. + */ + public ShooterWheelAdjust(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooterWheel.setSpeed(RobotContainer.shooterWheel.getTargetSpeed() + m_adjust); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjustVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjustVBus.java new file mode 100644 index 0000000..d9f3cb6 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjustVBus.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterWheelAdjustVBus extends InstantCommand { + double m_adjust; + + /** + * Creates a new ShooterAdjustWheelVBus. + */ + public ShooterWheelAdjustVBus(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double currentSpeed = RobotContainer.shooterWheel.getSpeedVBus(); + RobotContainer.shooterWheel.setSpeedVBus(currentSpeed + m_adjust); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java new file mode 100644 index 0000000..4cbef33 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterWheelSet extends CommandBase { + double m_rpm; + boolean m_waitForSpeed; + + public ShooterWheelSet(double rpm) { + this(rpm, false); + } + + /** + * Creates a new ShooterWheelSet + */ + public ShooterWheelSet(double rpm, boolean wait) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_rpm = rpm; + m_waitForSpeed = wait; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooterWheel.setSpeed(m_rpm); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if( m_waitForSpeed) + { + return (Math.abs( m_rpm - RobotContainer.shooterWheel.getSpeed() ) < 100); + } + else + { + return true; + } + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetVBus.java new file mode 100644 index 0000000..15b59ba --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetVBus.java @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterWheelSetVBus extends InstantCommand { + + double m_speed; + /** + * Creates a new ShooterSetWheelVBus. + */ + public ShooterWheelSetVBus(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_speed = speed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooterWheel.setSpeedVBus(m_speed); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java b/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java new file mode 100644 index 0000000..f55592a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretSetAbs extends CommandBase { + double m_setPoint; + + /** + * Creates a new TurretSetAbs. + */ + public TurretSetAbs(double setPoint) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.turret); + + m_setPoint = setPoint; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.turret.setPositionAbs(m_setPoint); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretSetRel.java b/src/main/java/org/mayheminc/robot2020/commands/TurretSetRel.java new file mode 100644 index 0000000..6433e88 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretSetRel.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretSetRel extends CommandBase { + double m_setPoint; + + /** + * Creates a new TurretSetRel. + */ + public TurretSetRel(double setPoint) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.turret); + + m_setPoint = setPoint; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.turret.setPositionRel(m_setPoint); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretSetVBus.java b/src/main/java/org/mayheminc/robot2020/commands/TurretSetVBus.java new file mode 100644 index 0000000..17ef5dd --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretSetVBus.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretSetVBus extends CommandBase { + double m_power; + + /** + * Creates a new TurretSetVBus. + */ + public TurretSetVBus(double power) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.turret); + m_power = power; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.turret.setVBus(m_power); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.turret.setVBus(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretZero.java b/src/main/java/org/mayheminc/robot2020/commands/TurretZero.java new file mode 100644 index 0000000..4a9e9fc --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretZero.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class TurretZero extends InstantCommand { + public TurretZero() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.turret.zero(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Feeder.java b/src/main/java/org/mayheminc/robot2020/subsystems/Feeder.java new file mode 100644 index 0000000..3e22bbf --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Feeder.java @@ -0,0 +1,47 @@ +package org.mayheminc.robot2020.subsystems; + +import org.mayheminc.robot2020.Constants; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Feeder extends SubsystemBase { + private final VictorSPX feederTalon = new VictorSPX(Constants.Talon.SHOOTER_FEEDER); + + /** + * Creates a new Feeder. + */ + public Feeder() { + + configureTalon(); + } + + public void init() { + configureTalon(); } + + private void configureTalon() { + feederTalon.setNeutralMode(NeutralMode.Brake); + feederTalon.configNominalOutputForward(+0.0f); + feederTalon.configNominalOutputReverse(0.0); + feederTalon.configPeakOutputForward(+12.0); + feederTalon.configPeakOutputReverse(-6.0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + UpdateDashboard(); + } + + private void UpdateDashboard() { + SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getMotorOutputVoltage()); + } + + public void setSpeed(double pos) { + feederTalon.set(ControlMode.PercentOutput, pos); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java new file mode 100644 index 0000000..f93d09c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java @@ -0,0 +1,122 @@ +package org.mayheminc.robot2020.subsystems; + +import org.mayheminc.robot2020.Constants; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.mayheminc.util.MayhemTalonSRX; +// import org.mayheminc.util.PidTuner; +import org.mayheminc.util.PidTunerObject; + +public class Hood extends SubsystemBase implements PidTunerObject { + private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); + + public final static double HOOD_TARGET_ZONE_POSITION = 5000; + public final static double HOOD_INITIATION_LINE_POSITION = 65000; + public final static double HOOD_TRENCH_MID_POSITION = 80000; + + /** + * Creates a new Hood. + */ + public Hood() { + configureTalon(); + } + + public void init() { + configureTalon(); + } + + private void configureTalon() { + hoodTalon.config_kP(0, 1.0, 0); + hoodTalon.config_kI(0, 0.0, 0); + hoodTalon.config_kD(0, 0.0, 0); + hoodTalon.config_kF(0, 0.0, 0); + + hoodTalon.changeControlMode(ControlMode.Position); + hoodTalon.setNeutralMode(NeutralMode.Coast); + hoodTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + hoodTalon.configNominalOutputVoltage(+0.0f, -0.0f); + hoodTalon.configPeakOutputVoltage(+12.0, -12.0); + hoodTalon.setInverted(true); + hoodTalon.setSensorPhase(true); + + hoodTalon.configForwardSoftLimitThreshold(100000); + hoodTalon.configForwardSoftLimitEnable(true); + hoodTalon.configReverseSoftLimitThreshold(0); + hoodTalon.configReverseSoftLimitEnable(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + UpdateDashboard(); + } + + private void UpdateDashboard() { + SmartDashboard.putNumber("Shooter Hood Pos", hoodTalon.getPosition()); + } + + public void zero() { + hoodTalon.setPosition(0); + } + + public void setPosition(double pos) { + hoodTalon.set(ControlMode.Position, pos); + } + + public double getPosition() { + return hoodTalon.getPosition(); + } + + public void setVBus(double d) { + hoodTalon.set(ControlMode.PercentOutput, d); + } + + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return hoodTalon.getP(); + } + + @Override + public double getI() { + return hoodTalon.getI(); + } + + @Override + public double getD() { + return hoodTalon.getD(); + } + + @Override + public double getF() { + return hoodTalon.getF(); + + } + + @Override + public void setP(double d) { + hoodTalon.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + hoodTalon.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + hoodTalon.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + hoodTalon.config_kF(0, d, 0); + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java new file mode 100644 index 0000000..7fb4c82 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -0,0 +1,187 @@ +package org.mayheminc.robot2020.subsystems; + +import org.mayheminc.robot2020.Constants; +// import org.mayheminc.robot2020.RobotContainer; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.PidTunerObject; + +public class ShooterWheel extends SubsystemBase implements PidTunerObject { + private final MayhemTalonSRX shooterWheelLeft = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_LEFT); + private final MayhemTalonSRX shooterWheelRight = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_RIGHT); + + // private final double MAX_SPEED_RPM = 5760.0; + private final double TALON_TICKS_PER_REV = 2048.0; + private final double SECONDS_PER_MINUTE = 60.0; + private final double HUNDRED_MS_PER_SECOND = 10.0; + + double m_targetSpeedRPM; + + // Note: for ease of thinking, 1 RPM =~ 3.4 native units for the shooter + double convertRpmToTicksPer100ms(double rpm) { + return rpm / SECONDS_PER_MINUTE * TALON_TICKS_PER_REV / HUNDRED_MS_PER_SECOND; + } + + // Note: 3.413 native units =~ 1.0 RPM for the shooter + double convertTicksPer100msToRPM(double ticks) { + return ticks * HUNDRED_MS_PER_SECOND / TALON_TICKS_PER_REV * SECONDS_PER_MINUTE; + } + + /** + * Creates a new Shooter. + */ + public ShooterWheel() { + configureWheelFalcons(); + } + + public void init() { + configureWheelFalcons(); + setSpeedVBus(0.0); + } + // configure a pair of shooter wheel falcons + private void configureWheelFalcons() { + // most of the configuration is shared for the two Falcons + configureOneWheelFalcon(shooterWheelLeft); + configureOneWheelFalcon(shooterWheelRight); + + // with the exception of one rotating the opposite direction + shooterWheelLeft.setInverted(false); + shooterWheelRight.setInverted(true); + } + private void configureOneWheelFalcon(MayhemTalonSRX shooterWheelFalcon) { + shooterWheelFalcon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); + shooterWheelFalcon.setNeutralMode(NeutralMode.Coast); + shooterWheelFalcon.configNominalOutputVoltage(+0.0f, -0.0f); + shooterWheelFalcon.configPeakOutputVoltage(+12.0, 0.0); + shooterWheelFalcon.configNeutralDeadband(0.001); // Config neutral deadband to be the smallest possible + + // For PIDF computations, 1023 is interpreted as "full" motor output + // Velocity is in native units of TicksPer100ms. + // 1000rpm =~ 3413 native units. + // P of "3.0" means that full power applied with error of 341 native units = 100rpm + // (above also means that 50% power boost applied with error of 50rpm) + shooterWheelFalcon.config_kP(0, 0.1, 0); // previously used 3.0 + shooterWheelFalcon.config_kI(0, 0.0, 0); + shooterWheelFalcon.config_kD(0, 0.0, 0); // CTRE recommends starting at 10x P-gain + shooterWheelFalcon.config_kF(0, 0.046, 0); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); + shooterWheelFalcon.configAllowableClosedloopError(0, 0, 0); // no "neutral" zone around target + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + UpdateDashboard(); + } + + + private void UpdateDashboard() { + SmartDashboard.putNumber("Shooter Wheel pos", shooterWheelLeft.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelLeft.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Shooter Wheel RPM", + convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0))); + + SmartDashboard.putNumber("Shooter Wheel target RPM", m_targetSpeedRPM); + SmartDashboard.putNumber("Shooter Wheel Error", + m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0))); + SmartDashboard.putNumber("Shooter Wheel Voltage", shooterWheelLeft.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter Wheel Bus Voltage", shooterWheelLeft.getBusVoltage()); + SmartDashboard.putNumber("Shooter Wheel Current", shooterWheelLeft.getSupplyCurrent()); + + SmartDashboard.putNumber("Shooter Wheel R-pos", shooterWheelRight.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Shooter Wheel R-speed", shooterWheelRight.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Shooter Wheel R-RPM", + convertTicksPer100msToRPM(shooterWheelRight.getSelectedSensorVelocity(0))); + + SmartDashboard.putNumber("Shooter Wheel R-target RPM", m_targetSpeedRPM); + SmartDashboard.putNumber("Shooter Wheel R-Error", + m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelRight.getSelectedSensorVelocity(0))); + SmartDashboard.putNumber("Shooter Wheel R-Voltage", shooterWheelRight.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter Wheel R-Bus Voltage", shooterWheelRight.getBusVoltage()); + SmartDashboard.putNumber("Shooter Wheel R-Current", shooterWheelRight.getSupplyCurrent()); + } + + public void zero() { + } + + /** + * Set shooter to rpm speed. + * + * @param rpm + */ + public void setSpeed(double rpm) { + m_targetSpeedRPM = rpm; + double ticks = convertRpmToTicksPer100ms(rpm); + shooterWheelLeft.set(ControlMode.Velocity, ticks); + shooterWheelRight.set(ControlMode.Velocity, ticks); + } + + public void setSpeedVBus(double pos) { + shooterWheelLeft.set(ControlMode.PercentOutput, pos); + shooterWheelRight.set(ControlMode.PercentOutput, pos); + } + + public double getSpeed() { + return convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0)); + } + + public double getTargetSpeed() { + return m_targetSpeedRPM; + } + + public double getSpeedVBus() { + return shooterWheelLeft.getMotorOutputVoltage(); + } + + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return shooterWheelLeft.getP(); + } + + @Override + public double getI() { + return shooterWheelLeft.getI(); + } + + @Override + public double getD() { + return shooterWheelLeft.getD(); + } + + @Override + public double getF() { + return shooterWheelLeft.getF(); + + } + + @Override + public void setP(double d) { + shooterWheelLeft.config_kP(0, d, 0); + shooterWheelRight.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + shooterWheelLeft.config_kI(0, d, 0); + shooterWheelRight.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + shooterWheelLeft.config_kD(0, d, 0); + shooterWheelRight.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + shooterWheelLeft.config_kF(0, d, 0); + shooterWheelRight.config_kF(0, d, 0); + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java new file mode 100644 index 0000000..e9b5c0e --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -0,0 +1,172 @@ +package org.mayheminc.robot2020.subsystems; + +import org.mayheminc.robot2020.Constants; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.mayheminc.util.History; +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.PidTunerObject; + +public class Turret extends SubsystemBase implements PidTunerObject { + private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); + + private final int MIN_POSITION = -12500; + private final int MAX_POSITION = +26000; + + double m_targetSpeedRPM; + History headingHistory = new History(); + + /** + * Creates a new Turret. + */ + public Turret() { + configureTurretTalon(); + } + + public void init() { + configureTurretTalon(); + } + + void configureTurretTalon() { + turretTalon.config_kP(0, 1.0, 0); + turretTalon.config_kI(0, 0.0, 0); + turretTalon.config_kD(0, 0.0, 0); + turretTalon.config_kF(0, 0.0, 0); + turretTalon.changeControlMode(ControlMode.Position); + turretTalon.setNeutralMode(NeutralMode.Coast); + turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + turretTalon.configMotionCruiseVelocity(800); // measured velocity of ~100K + // at 80%; set cruise to that + turretTalon.configMotionAcceleration(3200); // acceleration of 4x velocity + // allows cruise in 1/4 second + + turretTalon.configNominalOutputVoltage(+0.0f, -0.0f); + turretTalon.configPeakOutputVoltage(+6.0, -6.0); + + turretTalon.configForwardSoftLimitThreshold(MAX_POSITION); + turretTalon.configForwardSoftLimitEnable(true); + turretTalon.configReverseSoftLimitThreshold(MIN_POSITION); + turretTalon.configReverseSoftLimitEnable(true); + + this.setVBus(0.0); + } + + + + @Override + public void periodic() { + // This method will be called once per scheduler run + UpdateDashboard(); + updateHistory(); + } + + // KBS: tuned below at practice field on 21 Feb 2020 via successive refinement. + private static final double CAMERA_LAG = 0.08; // .05 was best so far in 2020; used .150 in 2019 + + private void updateHistory() { + double now = Timer.getFPGATimestamp(); + headingHistory.add(now, getPosition()); + } + + public double getAzimuthForCapturedImage() { + double now = Timer.getFPGATimestamp(); + double indexTime = now - CAMERA_LAG; + return headingHistory.getAzForTime(indexTime); + } + + private void UpdateDashboard() { + SmartDashboard.putNumber("Shooter turret pos", turretTalon.getPosition()); + SmartDashboard.putNumber("Shooter turret vbus", turretTalon.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter turret velocity", turretTalon.getSelectedSensorVelocity(0)); + } + + public void zero() { + turretTalon.setPosition(0); + } + + /** + * Set the absolute turret position. + */ + public void setPositionAbs(double pos) { + if (pos < MIN_POSITION) { + pos = MIN_POSITION; + } + if (pos > MAX_POSITION) { + pos = MAX_POSITION; + } + turretTalon.set(ControlMode.MotionMagic, pos); + } + + /** + * Set the relative turret position + * + * @param pos number of encoder ticks to adjust. + */ + public void setPositionRel(double pos) { + setPositionAbs(getPosition() + pos); + } + + public void setVBus(double power) { + turretTalon.set(ControlMode.PercentOutput, power); + } + + /** + * Get the current position of the turret. + * + * @return + */ + public double getPosition() { + return turretTalon.getPosition(); + } + + + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return turretTalon.getP(); + } + + @Override + public double getI() { + return turretTalon.getI(); + } + + @Override + public double getD() { + return turretTalon.getD(); + } + + @Override + public double getF() { + return turretTalon.getF(); + + } + + @Override + public void setP(double d) { + turretTalon.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + turretTalon.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + turretTalon.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + turretTalon.config_kF(0, d, 0); + } + +} \ No newline at end of file From 76c801f23ec4fa7aee1d7e5d70d147556849f2a5 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Wed, 26 Feb 2020 22:19:00 -0500 Subject: [PATCH 078/121] added safety init More robot tuning. Chimney has IR sensors at 1 and 2. Tweaked drive D value. Drive the pivot down slightly. --- src/main/java/frc/robot/Robot.java | 10 ++- .../org/mayheminc/robot2020/Constants.java | 4 +- .../mayheminc/robot2020/RobotContainer.java | 81 ++++++++++-------- .../autonomousroutines/TrenchAuto.java | 55 ------------- .../robot2020/commands/ChimneySetChimney.java | 43 ---------- .../robot2020/commands/HoodSetAbs.java | 8 +- .../robot2020/commands/IntakeSetPosition.java | 22 ++++- .../IntakeSetPositionWithoutWaiting.java | 38 --------- .../robot2020/commands/IntakeSetRollers.java | 9 +- .../robot2020/commands/MagazineDefault.java | 45 ---------- .../commands/MagazineSetTurntable.java | 43 ---------- .../commands/ShooterReadyAimFire.java | 29 ++++--- .../robot2020/commands/ShooterWheelSet.java | 6 +- .../robot2020/commands/TurretSetAbs.java | 12 ++- .../robot2020/subsystems/Chimney.java | 6 +- .../mayheminc/robot2020/subsystems/Drive.java | 82 +++++++++---------- .../mayheminc/robot2020/subsystems/Hood.java | 16 +++- .../robot2020/subsystems/Intake.java | 9 +- .../robot2020/subsystems/Magazine.java | 57 ------------- .../robot2020/subsystems/ShooterWheel.java | 3 + .../robot2020/subsystems/Turret.java | 49 ++++++++--- 21 files changed, 216 insertions(+), 411 deletions(-) delete mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/ChimneySetChimney.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/IntakeSetPositionWithoutWaiting.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java delete mode 100644 src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java delete mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b37972a..0aac71e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -35,7 +35,7 @@ public class Robot extends TimedRobot { @Override @SuppressWarnings("deprecation") public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, + // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); @@ -81,6 +81,9 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { + // ensure robot "safety" before anything else when entering new mode + RobotContainer.safetyInit(); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) @@ -98,6 +101,9 @@ public void autonomousPeriodic() { @Override public void teleopInit() { + // ensure robot "safety" before anything else when entering new mode + RobotContainer.safetyInit(); + // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -106,7 +112,7 @@ public void teleopInit() { m_autonomousCommand.cancel(); } - // RobotContainer.compressor.start(); + // for safety reasons, call } /** diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index e7c41cd..3362fd9 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -34,8 +34,8 @@ public final class Talon { public static final int INTAKE_ROLLERS = 9; public static final int INTAKE_PIVOT = 10; - public static final int MAGAZINE_TURNTABLE = 11; - public static final int MAGAZINE_CHIMNEY = 12; + public static final int REVOLVER_TURNTABLE = 11; + public static final int CHIMNEY_ROLLER = 12; public static final int CLIMBER_WINCH_LEFT = 14; // high current public static final int CLIMBER_WINCH_RIGHT = 13; // high current diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 9d3818d..2fa2fdf 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -32,7 +32,7 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... public static final Climber climber = new Climber(); - public static final Magazine magazine = new Magazine(); + public static final Revolver revolver = new Revolver(); public static final ShooterWheel shooterWheel = new ShooterWheel(); public static final Hood hood = new Hood(); public static final Turret turret = new Turret(); @@ -74,33 +74,50 @@ public static void init() { shooterWheel.init(); } + public static void safetyInit() { + hood.setVBus(0.0); + turret.setVBus(0.0); + climber.setPistons(false); + } + private void configureDefaultCommands() { drive.setDefaultCommand(new DriveDefault()); // intake.setDefaultCommand(new IntakeExtenderVBus()); - magazine.setDefaultCommand(new MagazineDefault()); - - // TODO: Figure out if the current approach of "AirCompressorDefault()" is the way to go for compressor control. - // KBS doesn't think the below is the right way to have the compressor be on "by default" because - // it would require there to always be a command running to keep the compressor off. However, that - // is a good way to ensure it doesn't get left off by accident. Not quite sure how to handle this; - // would really rather that other commands which need the compressor off (such as a high-power command - // which wants all the battery power available) would turn the compressor off when the command starts - // and off when the command ends.) Then again, maybe the "defaultCommand" is a good way to do this + revolver.setDefaultCommand(new RevolverDefault()); + + // TODO: Figure out if the current approach of "AirCompressorDefault()" is the + // way to go for compressor control. + // KBS doesn't think the below is the right way to have the compressor be on "by + // default" because + // it would require there to always be a command running to keep the compressor + // off. However, that + // is a good way to ensure it doesn't get left off by accident. Not quite sure + // how to handle this; + // would really rather that other commands which need the compressor off (such + // as a high-power command + // which wants all the battery power available) would turn the compressor off + // when the command starts + // and off when the command ends.) Then again, maybe the "defaultCommand" is a + // good way to do this // and I just don't understand the style yet. // compressor.setDefaultCommand(new AirCompressorDefault()); } private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - // TODO: fix "wierdness" with auto program selection - sometimes doesn't seem to work - // TODO: fix so that auto program is shown not just when changed (as shows old setting sometimes) - - // autonomousPrograms.push(/* 01 */ new StayStill()); - autonomousPrograms.push(/* 01 */ new DriveStraightOnly()); - autonomousPrograms.push(/* 00 */ new TrenchAuto()); - // autonomousPrograms.push( new ShooterReadyAimFire()); - // autonomousPrograms.push(new TestTurret()); + autonomousPrograms.push(/* 11 */ new StayStill()); + autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToRP()); + autonomousPrograms.push(/* 09 */ new StartBWDriveOnlyToWall()); + autonomousPrograms.push(/* 08 */ new StartFWDriveOnlyToRP()); + autonomousPrograms.push(/* 07 */ new StartFWDriveOnlyToWall()); + autonomousPrograms.push(/* 06 */ new StartBWShoot3ThenToRP()); + autonomousPrograms.push(/* 05 */ new StartBWShoot3ThenToWall()); + autonomousPrograms.push(/* 04 */ new StartFWShoot3ThenToRP()); + autonomousPrograms.push(/* 03 */ new StartFWShoot3ThenToWall()); + autonomousPrograms.push(/* 02 */ new StartFWRendezvous()); + autonomousPrograms.push(/* 01 */ new StartBWOppTrench()); + autonomousPrograms.push(/* 00 */ new StartBWTrench()); autonomous.setAutonomousPrograms(autonomousPrograms); @@ -160,8 +177,8 @@ private void configureDriverPadButtons() { // about -30 degrees // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new // ShooterSetTurretVBus(+0.2));// about +30 degrees - DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbs(Hood.HOOD_INITIATION_LINE_POSITION)); - DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbs(Hood.HOOD_TARGET_ZONE_POSITION)); + DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION)); + DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new // ShooterSetTurretRel(-200.0)); @@ -184,7 +201,8 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(3000)); DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1)); - // TODO: above hard-coded constant (3000) should be a named constant from Shooter.java + // TODO: above hard-coded constant (3000) should be a named constant from + // Shooter.java DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0)); @@ -194,28 +212,27 @@ private void configureOperatorStickButtons() { } private void configureOperatorPadButtons() { - OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new MagazineSetTurntable(0.2)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new RevolverSetTurntable(0.2)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new MagazineSetTurntable(0.5)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(0.5)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // new ShooterSetWheel(1000)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySetChimney(1.0)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollers(-1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySet(1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollersWhileHeld(-1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whileHeld(new TurretAimToTarget()); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollers(1.0)); - + OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollersWhileHeld(1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(true)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_TEN.whenPressed(new ClimberSetPistons(false)); - // OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new // IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new TurretSetVBus(-0.2)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new TurretSetVBus(+0.2)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new TurretSetVBus(-0.4)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new TurretSetVBus(+0.4)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new HoodAdjust(+1000.0)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new HoodAdjust(-1000.0)); @@ -223,8 +240,8 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-1.0)); // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new - // MagazineSetTurntable()); - OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whileHeld(new ChimneySetChimney(-1.0)); + // RevolverSetTurntable()); + OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whileHeld(new ChimneySet(-1.0)); } /** diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java deleted file mode 100644 index 520a321..0000000 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java +++ /dev/null @@ -1,55 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.autonomousroutines; - -import org.mayheminc.robot2020.RobotContainer; -import org.mayheminc.robot2020.commands.*; -import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; -import org.mayheminc.robot2020.subsystems.Hood; - -import edu.wpi.first.wpilibj2.command.*; - -public class TrenchAuto extends SequentialCommandGroup { - /** - * Add your docs here. - */ - public TrenchAuto() { - - addCommands(new DriveZeroGyro(180.0)); - // first, shoot the balls that were pre-loaded - - addCommands(new IntakeSetPositionWithoutWaiting(RobotContainer.intake.PIVOT_DOWN)); - addCommands(new TurretSetAbs(+23000.0)); - addCommands(new ShooterWheelSet(3000.0, false)); - addCommands(new Wait(2.0)); - addCommands(new ShooterReadyAimFire(2.5)); - - // then, drive down the trench, jogging left a little - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 36, 150)); - - // pick up balls while heading down the trench. - addCommands(new ParallelRaceGroup( - // intake while driving down the trench - new IntakeSetRollers(-1.0), - new SequentialCommandGroup(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 180, 180), - new Wait(0.5), new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 180)))); - - // after getting all three balls, go back to shooting position - addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 48, 180)); - addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 96, 160)); - addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 24, 180)); - - addCommands(new ShooterReadyAimFire(6.0)); - - // turn the wheel off now that the shooting is all done - addCommands(new ShooterWheelSet(0.0)); - - // turn the wheel off now that the shooting is all done - addCommands(new HoodSetAbs(Hood.HOOD_TARGET_ZONE_POSITION)); - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ChimneySetChimney.java b/src/main/java/org/mayheminc/robot2020/commands/ChimneySetChimney.java deleted file mode 100644 index 1d7cc72..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/ChimneySetChimney.java +++ /dev/null @@ -1,43 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ChimneySetChimney extends CommandBase { - double m_speed; - - /** - * Creates a new MagazineSetChimney. - */ - public ChimneySetChimney(double d) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.chimney); - m_speed = d; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.chimney.setChimneySpeed(m_speed); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - RobotContainer.chimney.setChimneySpeed(0.0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java index 9b142a2..169b9e7 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java @@ -9,9 +9,9 @@ import org.mayheminc.robot2020.RobotContainer; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.CommandBase; -public class HoodSetAbs extends InstantCommand { +public class HoodSetAbs extends CommandBase { double m_set; /** @@ -30,4 +30,8 @@ public void initialize() { RobotContainer.hood.setPosition(m_set); } + @Override + public boolean isFinished() { + return (RobotContainer.hood.isAtPosition()); + } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java index 1e2e318..16f8847 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java @@ -13,17 +13,27 @@ public class IntakeSetPosition extends CommandBase { double m_position; + boolean m_waitForDone; /** - * Creates a new IntakeSetPosition. + * Creates a new IntakeSetPosition, with "wait" set to false */ - public IntakeSetPosition(Double position) { + public IntakeSetPosition(double position) { + this (position, false); + } + + /** + * Creates a new IntakeSetPosition + */ + public IntakeSetPosition(double position, boolean wait) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.intake); - m_position = position; + m_position = position; + m_waitForDone = wait; } + // Called when the command is initially scheduled. @Override public void initialize() { @@ -33,6 +43,10 @@ public void initialize() { // Returns true when the command should end. @Override public boolean isFinished() { - return RobotContainer.intake.isPivotAtPosition(); + if (m_waitForDone) { + return RobotContainer.intake.isPivotAtPosition(); + } else { + return true; + } } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPositionWithoutWaiting.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPositionWithoutWaiting.java deleted file mode 100644 index e5f9c7f..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPositionWithoutWaiting.java +++ /dev/null @@ -1,38 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class IntakeSetPositionWithoutWaiting extends CommandBase { - double m_position; - - /** - * Creates a new IntakeSetPositionWithoutWaiting. - */ - public IntakeSetPositionWithoutWaiting(Double position) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.intake); - m_position = position; - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.intake.setPivot(m_position); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java index 56b2e0c..c1d8a7a 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java @@ -21,7 +21,6 @@ public IntakeSetRollers(double speed) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.intake); m_speed = speed; - } // Called when the command is initially scheduled. @@ -30,15 +29,9 @@ public void initialize() { RobotContainer.intake.setRollers(m_speed); } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - RobotContainer.intake.setRollers(0.0); - } - // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java deleted file mode 100644 index 2310374..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineDefault.java +++ /dev/null @@ -1,45 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class MagazineDefault extends CommandBase { - /** - * Creates a new MagazineDefault. - */ - public MagazineDefault() { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.magazine); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - RobotContainer.magazine.setTurntableSpeed(-RobotContainer.OPERATOR_PAD.getLeftYAxis()); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java b/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java deleted file mode 100644 index a0de4ea..0000000 --- a/src/main/java/org/mayheminc/robot2020/commands/MagazineSetTurntable.java +++ /dev/null @@ -1,43 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.commands; - -import org.mayheminc.robot2020.RobotContainer; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class MagazineSetTurntable extends CommandBase { - double m_speed; - - /** - * Creates a new MagazineSetTurntable. - */ - public MagazineSetTurntable(double d) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(RobotContainer.magazine); - m_speed = d; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - RobotContainer.magazine.setTurntableSpeed(m_speed); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - RobotContainer.magazine.setTurntableSpeed(0.0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index 9f1c77e..a87c5b9 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -24,23 +24,28 @@ public ShooterReadyAimFire(double waitDuration) { // super(new FooCommand(), new BarCommand()); super(); - addCommands(new HoodSetAbs(Hood.HOOD_INITIATION_LINE_POSITION)); - // Turn off compressor while actively shooting. addCommands(new AirCompressorPause()); - // aim to the target until we are on target. - addCommands( - new ParallelRaceGroup(new ParallelCommandGroup(new TargetingIsOnTarget(), new ShooterWheelSet(3000.0, true)), - new TurretAimToTarget())); + // aim to the target before starting shooting. + // don't continue to next command (to actually start) until both onTarget and up + // to speed + addCommands(new ParallelRaceGroup( + new ParallelCommandGroup(/* new TargetingIsOnTarget(), */ new ShooterWheelSet(3000.0, true)), + new TurretAimToTarget())); // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the - // magazine, shoot for about 4 seconds - addCommands(new ParallelRaceGroup(new FeederSet(1.0), - new SequentialCommandGroup(new Wait(0.1), new ChimneySetChimney(0.5)), - new SequentialCommandGroup(new Wait(0.2), new MagazineSetTurntable(0.3)), new Wait(waitDuration))); + // revolver turntable, shoot for specified duration + // TODO: should really shoot until no balls detected any more + addCommands( + new ParallelRaceGroup(new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), new Wait(waitDuration))); + + // turn off the feeder, chimney, and revolver, ending after 0.1 seconds + addCommands( + new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0), new Wait(0.1))); - addCommands(new ParallelRaceGroup(new MagazineSetTurntable(0.0), new ChimneySetChimney(0.0), - new FeederSet(0.0), new Wait(0.1))); + // Lower the hood now that we're done shooting + addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_STARTING_POSITION)); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java index 4cbef33..ae0ef83 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java @@ -13,7 +13,7 @@ public class ShooterWheelSet extends CommandBase { double m_rpm; - boolean m_waitForSpeed; + boolean m_waitForDone; public ShooterWheelSet(double rpm) { this(rpm, false); @@ -27,7 +27,7 @@ public ShooterWheelSet(double rpm, boolean wait) { addRequirements(RobotContainer.shooterWheel); m_rpm = rpm; - m_waitForSpeed = wait; + m_waitForDone = wait; } // Called when the command is initially scheduled. @@ -39,7 +39,7 @@ public void initialize() { // Returns true when the command should end. @Override public boolean isFinished() { - if( m_waitForSpeed) + if( m_waitForDone) { return (Math.abs( m_rpm - RobotContainer.shooterWheel.getSpeed() ) < 100); } diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java b/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java index f55592a..99d14a0 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java @@ -13,15 +13,21 @@ public class TurretSetAbs extends CommandBase { double m_setPoint; + boolean m_waitForDone; /** * Creates a new TurretSetAbs. */ public TurretSetAbs(double setPoint) { + this(setPoint, false); + } + + public TurretSetAbs(double setPoint, boolean wait) { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.turret); m_setPoint = setPoint; + m_waitForDone = wait; } // Called when the command is initially scheduled. @@ -33,6 +39,10 @@ public void initialize() { // Returns true when the command should end. @Override public boolean isFinished() { - return true; + if (m_waitForDone) { + return RobotContainer.turret.isAtDesiredPosition(); + } else { + return true; + } } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java index a21eac8..9916316 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -18,9 +18,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Chimney extends SubsystemBase { - private final VictorSPX chimneyTalon = new VictorSPX(Constants.Talon.MAGAZINE_CHIMNEY); - private final RangeFinder_GP2D120 frontIR = new RangeFinder_GP2D120(2, 0); - private final RangeFinder_GP2D120 middleIR = new RangeFinder_GP2D120(3, 0); + private final VictorSPX chimneyTalon = new VictorSPX(Constants.Talon.CHIMNEY_ROLLER); + private final RangeFinder_GP2D120 frontIR = new RangeFinder_GP2D120(1, 0); + private final RangeFinder_GP2D120 middleIR = new RangeFinder_GP2D120(2, 0); /** * Creates a new Chimney. diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 3b05bc4..a5392f0 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -52,6 +52,7 @@ public class Drive extends SubsystemBase implements PidTunerObject { private double m_desiredHeading = 0.0; private boolean m_useHeadingCorrection = true; private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR; was 0.007 in early 2020 + private static final double HEADING_PID_D = 0.080; // was 0.04 in 2019 private static final double kToleranceDegreesPIDControl = 0.2; // Drive parameters @@ -96,7 +97,7 @@ public Drive() { m_HeadingError = new PIDHeadingError(); m_HeadingError.m_Error = 0.0; m_HeadingCorrection = new PIDHeadingCorrection(); - m_HeadingPid = new PIDController(HEADING_PID_P, 0.000, 0.04, m_HeadingError, m_HeadingCorrection, + m_HeadingPid = new PIDController(HEADING_PID_P, 0.000, HEADING_PID_D, m_HeadingError, m_HeadingCorrection, 0.020 /* period in seconds */); m_HeadingPid.setInputRange(-180.0f, 180.0f); m_HeadingPid.setContinuous(true); // treats the input range as "continous" with wrap-around @@ -737,54 +738,51 @@ public void setDesiredHeading(final double desiredHeading) { resetAndEnableHeadingPID(); } + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return leftFrontTalon.getP(); + } - - //////////////////////////////////////////////////// - // PidTunerObject - @Override - public double getP() { - return leftFrontTalon.getP(); - } - - @Override - public double getI() { - return leftFrontTalon.getI(); - } - - @Override - public double getD() { - return leftFrontTalon.getD(); - } + @Override + public double getI() { + return leftFrontTalon.getI(); + } - @Override - public double getF() { - return leftFrontTalon.getF(); + @Override + public double getD() { + return leftFrontTalon.getD(); + } - } + @Override + public double getF() { + return leftFrontTalon.getF(); - @Override - public void setP(double d) { - leftFrontTalon.config_kP(0, d, 0); - rightFrontTalon.config_kP(0, d, 0); - } + } - @Override - public void setI(double d) { - leftFrontTalon.config_kI(0, d, 0); - rightFrontTalon.config_kI(0, d, 0); - } + @Override + public void setP(double d) { + leftFrontTalon.config_kP(0, d, 0); + rightFrontTalon.config_kP(0, d, 0); + } - @Override - public void setD(double d) { - leftFrontTalon.config_kD(0, d, 0); - rightFrontTalon.config_kD(0, d, 0); - } + @Override + public void setI(double d) { + leftFrontTalon.config_kI(0, d, 0); + rightFrontTalon.config_kI(0, d, 0); + } - @Override - public void setF(double d) { - leftFrontTalon.config_kF(0, d, 0); - rightFrontTalon.config_kF(0, d, 0); - } + @Override + public void setD(double d) { + leftFrontTalon.config_kD(0, d, 0); + rightFrontTalon.config_kD(0, d, 0); + } + @Override + public void setF(double d) { + leftFrontTalon.config_kF(0, d, 0); + rightFrontTalon.config_kF(0, d, 0); + } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java index f93d09c..f276ea4 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java @@ -14,10 +14,15 @@ public class Hood extends SubsystemBase implements PidTunerObject { private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); + public final static double HOOD_STARTING_POSITION = 0; public final static double HOOD_TARGET_ZONE_POSITION = 5000; public final static double HOOD_INITIATION_LINE_POSITION = 65000; public final static double HOOD_TRENCH_MID_POSITION = 80000; + private final static double POSITION_TOLERANCE = 1000.0; + + private double m_desiredPosition = 0.0; + /** * Creates a new Hood. */ @@ -47,7 +52,7 @@ private void configureTalon() { hoodTalon.configForwardSoftLimitThreshold(100000); hoodTalon.configForwardSoftLimitEnable(true); hoodTalon.configReverseSoftLimitThreshold(0); - hoodTalon.configReverseSoftLimitEnable(false); + hoodTalon.configReverseSoftLimitEnable(true); } @Override @@ -62,10 +67,17 @@ private void UpdateDashboard() { public void zero() { hoodTalon.setPosition(0); + m_desiredPosition = 0.0; + hoodTalon.set(ControlMode.PercentOutput, 0.0); } public void setPosition(double pos) { - hoodTalon.set(ControlMode.Position, pos); + m_desiredPosition = pos; + hoodTalon.set(ControlMode.Position, m_desiredPosition); + } + + public boolean isAtPosition() { + return (Math.abs(getPosition() - m_desiredPosition) < POSITION_TOLERANCE); } public double getPosition() { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 5d30301..e1f2213 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -74,7 +74,8 @@ void configPivotMotor(MayhemTalonSRX motor) { motor.setNeutralMode(NeutralMode.Coast); motor.setInverted(false); - // in general, sensor phase inversion needed for gearboxes which reverse sensor direction due to odd number of stages + // in general, sensor phase inversion needed for gearboxes which reverse sensor + // direction due to odd number of stages motor.setSensorPhase(true); motor.configNominalOutputVoltage(+0.0f, -0.0f); motor.configPeakOutputVoltage(+12.0, -12.0); @@ -100,7 +101,6 @@ public void setRollers(double power) { public void setPivot(Double b) { m_targetPosition = b; - mode = PivotMode.PID_MODE; isMoving = true; } @@ -121,11 +121,10 @@ public void periodic() { private void updatePivotPower() { if (mode == PivotMode.PID_MODE) { - - // if the pivot is close enough, turn off the motor + // if the pivot is close enough, turn the motor on gently downards if (Math.abs(pivotTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) { isMoving = false; - setPivotVBus(0); + setPivotVBus(-0.05); } else { pivotTalon.set(ControlMode.Position, m_targetPosition, DemandType.ArbitraryFeedForward, m_feedForward); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java b/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java deleted file mode 100644 index cc74ef8..0000000 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Magazine.java +++ /dev/null @@ -1,57 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.subsystems; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.NeutralMode; - -import org.mayheminc.robot2020.Constants; -import org.mayheminc.util.MayhemTalonSRX; - -public class Magazine extends SubsystemBase { - private final MayhemTalonSRX turntableTalon = new MayhemTalonSRX(Constants.Talon.MAGAZINE_TURNTABLE); - - /** - * Creates a new Magazine. - */ - public Magazine() { - ConfigureTalon(turntableTalon); - turntableTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - } - - private void ConfigureTalon(MayhemTalonSRX talon) { - talon.setNeutralMode(NeutralMode.Coast); - talon.configNominalOutputVoltage(+0.0f, -0.0f); - talon.configPeakOutputVoltage(+12.0, -12.0); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - updateSmartDashboard(); - monitorTurntableMovement(); - } - - void updateSmartDashboard() { - SmartDashboard.putNumber("Magazine Speed", turntableTalon.getSpeed()); - // SmartDashboard.putNumber("Magazine Speed", turntableTalon.getSpeed()); - } - - void monitorTurntableMovement() { - - } - - public void setTurntableSpeed(double speed) { - turntableTalon.set(ControlMode.PercentOutput, speed); - } - -} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java index 7fb4c82..292584c 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -20,6 +20,9 @@ public class ShooterWheel extends SubsystemBase implements PidTunerObject { private final double SECONDS_PER_MINUTE = 60.0; private final double HUNDRED_MS_PER_SECOND = 10.0; + public static final double SHOOTER_WHEEL_INITIATION_LINE_SPEED = 3000.0; + public static final double SHOOTER_WHEEL_TRENCH_FRONT_SPEED = 3400.0; + double m_targetSpeedRPM; // Note: for ease of thinking, 1 RPM =~ 3.4 native units for the shooter diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java index e9b5c0e..42c990d 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -15,10 +15,19 @@ public class Turret extends SubsystemBase implements PidTunerObject { private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); - private final int MIN_POSITION = -12500; - private final int MAX_POSITION = +26000; - - double m_targetSpeedRPM; + public static final boolean WAIT_FOR_DONE = true; + private final int MIN_POSITION = -12500; // approx 90 degrees + private final int MAX_POSITION = +26000; // approx 180 degrees + private final int DESTINATION_TOLERANCE = 200; + // TICKS_PER_DEGREE computed by 4096 ticks per rotation of shaft / 1 rotation of + // shaft per 18t * 225t / 1 rotation of turret + + public final static double TICKS_PER_DEGREE = 4096.0 / 18.0 * 225.0 / 360.0; + // above works out to 142.2 ticks per degree + // a full circle is 51,200 ticks + + double m_targetSpeedRPM = 0.0; + double m_desiredPosition = 0.0; History headingHistory = new History(); /** @@ -41,13 +50,11 @@ void configureTurretTalon() { turretTalon.setNeutralMode(NeutralMode.Coast); turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - turretTalon.configMotionCruiseVelocity(800); // measured velocity of ~100K - // at 80%; set cruise to that - turretTalon.configMotionAcceleration(3200); // acceleration of 4x velocity - // allows cruise in 1/4 second + turretTalon.configMotionCruiseVelocity(2500); // max velocity at 100% is about 3200, cruise at 80% + turretTalon.configMotionAcceleration(5000); // acceleration of 2x velocity, allows cruise in 1/2 second turretTalon.configNominalOutputVoltage(+0.0f, -0.0f); - turretTalon.configPeakOutputVoltage(+6.0, -6.0); + turretTalon.configPeakOutputVoltage(+12.0, -12.0); turretTalon.configForwardSoftLimitThreshold(MAX_POSITION); turretTalon.configForwardSoftLimitEnable(true); @@ -57,8 +64,6 @@ void configureTurretTalon() { this.setVBus(0.0); } - - @Override public void periodic() { // This method will be called once per scheduler run @@ -87,6 +92,7 @@ private void UpdateDashboard() { } public void zero() { + m_desiredPosition = 0.0; turretTalon.setPosition(0); } @@ -100,6 +106,7 @@ public void setPositionAbs(double pos) { if (pos > MAX_POSITION) { pos = MAX_POSITION; } + m_desiredPosition = pos; turretTalon.set(ControlMode.MotionMagic, pos); } @@ -109,7 +116,8 @@ public void setPositionAbs(double pos) { * @param pos number of encoder ticks to adjust. */ public void setPositionRel(double pos) { - setPositionAbs(getPosition() + pos); + m_desiredPosition = getPosition() + pos; + setPositionAbs(m_desiredPosition); } public void setVBus(double power) { @@ -125,6 +133,23 @@ public double getPosition() { return turretTalon.getPosition(); } + /** + * Get the desired position of the turret. + * + * @return + */ + public double getDesiredPosition() { + return m_desiredPosition; + } + + /** + * Return true if close enough to desired position + * + * @return + */ + public boolean isAtDesiredPosition() { + return (Math.abs(getPosition() - getDesiredPosition()) < DESTINATION_TOLERANCE); + } //////////////////////////////////////////////////// // PidTunerObject From afdeefeaa43389110133b46e89d830b12d4ab4ab Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Wed, 26 Feb 2020 22:20:06 -0500 Subject: [PATCH 079/121] adding new autonomous routines parts. Adding more commands. --- .../StartBWDriveOnlyToRP.java | 30 ++++++++ .../StartBWDriveOnlyToWall.java | 30 ++++++++ .../autonomousroutines/StartBWOppTrench.java | 63 ++++++++++++++++ .../autonomousroutines/StartBWShoot3.java | 42 +++++++++++ .../StartBWShoot3ThenToRP.java | 26 +++++++ .../StartBWShoot3ThenToWall.java | 27 +++++++ .../autonomousroutines/StartBWTrench.java | 73 ++++++++++++++++++ .../StartFWDriveOnlyToRP.java | 30 ++++++++ .../StartFWDriveOnlyToWall.java | 30 ++++++++ .../autonomousroutines/StartFWRendezvous.java | 75 +++++++++++++++++++ .../autonomousroutines/StartFWShoot3.java | 42 +++++++++++ .../StartFWShoot3ThenToRP.java | 26 +++++++ .../StartFWShoot3ThenToWall.java | 26 +++++++ .../robot2020/commands/ChimneySet.java | 43 +++++++++++ .../commands/HoodSetAbsWhileHeld.java | 33 ++++++++ .../commands/IntakeSetRollersWhileHeld.java | 44 +++++++++++ .../robot2020/commands/RevolverDefault.java | 45 +++++++++++ .../commands/RevolverSetTurntable.java | 43 +++++++++++ .../commands/ShooterPermissionToFire.java | 56 ++++++++++++++ .../robot2020/subsystems/Revolver.java | 52 +++++++++++++ 20 files changed, 836 insertions(+) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToRP.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToWall.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToWall.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ChimneySet.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollersWhileHeld.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/RevolverDefault.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/RevolverSetTurntable.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java create mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java new file mode 100644 index 0000000..d6b7ef3 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWDriveOnlyToRP extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWDriveOnlyToRP() { + + // start backwards + addCommands(new DriveZeroGyro(180.0)); + + // lower the intake + addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12, 180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java new file mode 100644 index 0000000..8978dbd --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWDriveOnlyToWall extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWDriveOnlyToWall() { + + // start backwards + addCommands(new DriveZeroGyro(180.0)); + + // lower the intake + addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + + // then, drive towards the wall + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 40, 180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java new file mode 100644 index 0000000..863ad54 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWOppTrench extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWOppTrench() { + + addCommands(new DriveZeroGyro(180.0)); + + // lower the intake and turn it on before driving forwards + addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + addCommands(new IntakeSetRollers(-1.0)); + + // make sure the hood is down + addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); + + // drive to get balls from opponent's trench + addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 108, 180)); + + // now driven to get the balls from opposing trench + addCommands(new Wait(0.8), // wait for last two balls to get into robot + new IntakeSetRollers(0), // turn off the intake + new DriveStraightOnHeading(-0.4, DistanceUnits.INCHES, 36, 180)); // start backing up + // slowly + + // backup about halfway across the field + addCommands(new DriveStraightOnHeading(-0.4, DistanceUnits.INCHES, 180, 270)); + + // in shooting position, prepare everything for shooting + addCommands(new ParallelCommandGroup( // run the following commands in parallel: + new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION + 3000.0), + new TurretSetAbs((105.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + // turn on the intake gently while shooting to help balls settle + addCommands(new IntakeSetRollers(-0.2)); + addCommands(new ShooterReadyAimFire(5.0)); + + // turn the shooter wheel and intake off now that the shooting is all done + addCommands(new ShooterWheelSet(0.0)); + addCommands(new IntakeSetRollers(0.0)); + + // turn the wheel off now that the shooting is all done + addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java new file mode 100644 index 0000000..2e5a3a1 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java @@ -0,0 +1,42 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWShoot3 extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWShoot3() { + + addCommands(new DriveZeroGyro(180.0)); + + // first, lower the intake, start the shooter wheel, and wait until the turret + // is turned towards the target + addCommands(new ParallelCommandGroup( // run the following commands in parallel: + new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), + new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION), + new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + addCommands(new ShooterReadyAimFire(1.5)); + + // turn the shooter wheel and intake off now that the shooting is all done + addCommands(new ParallelCommandGroup( // below commands in parallel + new ShooterWheelSet(0.0), // + new IntakeSetRollers(0.0), // turn off the rollers + new HoodSetAbs(Hood.HOOD_STARTING_POSITION))); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToRP.java new file mode 100644 index 0000000..4c10f54 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToRP.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWShoot3ThenToRP extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWShoot3ThenToRP() { + + // start backwards and shoot the first three balls + addCommands(new StartBWShoot3()); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12, 180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToWall.java new file mode 100644 index 0000000..f4687e4 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToWall.java @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWShoot3ThenToWall extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWShoot3ThenToWall() { + + // start backwards and shoot the first three balls + addCommands(new StartBWShoot3()); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, -40, 180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java new file mode 100644 index 0000000..f91eb08 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWTrench extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWTrench() { + + // start backwards and shoot the first three balls + addCommands(new StartBWShoot3()); + + // then, drive to the trench entrance (jog left a little to get there) + addCommands(new DriveStraightOnHeading(0.5, DistanceUnits.INCHES, 40, 150)); + + // pick up balls while heading down the trench. + addCommands(new ParallelCommandGroup( + // intake while driving down the trench + new IntakeSetRollers(-1.0), + // ensure the hood is down + new HoodSetAbsWhileHeld(Hood.HOOD_STARTING_POSITION), + // drive the path under the control panel to the end + new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 180, 180))); + + // now driven to the balls at far end of trench + addCommands(new Wait(0.8), // wait for last two balls to get into robot + new IntakeSetRollers(0), // turn off the intake + new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 12, 180)); // start backing up + // slowly + + // after getting all three balls, go back to shooting position + // first, make sure we drive straight out from under the control panel + addCommands(new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 48, 180)); + + // drive diagonally over towards the shooting position, while turning on shooter + // wheels, raising the hood, and re-aiming the turret + addCommands(new ParallelCommandGroup( + new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION), + new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)), + new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160))); + + // straighten out again to enable turret to aim to the target + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 24, 180)); + + // turn on the intake gently while shooting to help balls settle + addCommands(new IntakeSetRollers(-0.2)); + + // start the shooting sequence + addCommands(new ShooterReadyAimFire(6.0)); + + // turn the shooter wheel and intake off now that the shooting is all done + addCommands(new ShooterWheelSet(0.0)); + addCommands(new IntakeSetRollers(0.0)); + + // turn the wheel off now that the shooting is all done + addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java new file mode 100644 index 0000000..231b8b1 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWDriveOnlyToRP extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWDriveOnlyToRP() { + + // start forwards + addCommands(new DriveZeroGyro(0.0)); + + // lower the intake + addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + + // then, drive to the RP + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java new file mode 100644 index 0000000..972963a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWDriveOnlyToWall extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWDriveOnlyToWall() { + + // start forwards + addCommands(new DriveZeroGyro(0.0)); + + // lower the intake + addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + + // then, drive to the RP + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 40, 0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java new file mode 100644 index 0000000..84bd34c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWRendezvous extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWRendezvous() { + + // start out facing in the normal direction + addCommands(new DriveZeroGyro(0.0)); + + // shoot the 3 balls we started with + // first, lower the intake, start the shooter wheel, and wait until the turret + // is turned towards the target + addCommands(new ParallelCommandGroup( // run the following commands in parallel: + new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), + new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + new HoodSetAbs(Hood.HOOD_INITIATION_LINE_POSITION), + new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + addCommands(new ShooterReadyAimFire(1.0)); + + // drive a little bit out to get away from other robots. + addCommands(new DriveStraightOnHeading(-0.2, DistanceUnits.INCHES, 36, 30)); + + // now that we are clear of other robots, lower the intake while backing up + // further + addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + + // rais the hood a little to shoot from this increased distance + addCommands(new HoodSetAbsWhileHeld( + (Hood.HOOD_INITIATION_LINE_POSITION + Hood.HOOD_TRENCH_MID_POSITION) / 2.0)); + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 144, 50)); + + // turn on the intake to pick up balls + addCommands(new IntakeSetRollers(-1.0)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 84, 30)); + + // now driven to get the balls, stay here and shoot them + addCommands(new Wait(0.8), // wait for the balls to get into robot + new IntakeSetRollers(0)); // turn off the intake + + // in shooting position, prepare everything for shooting + addCommands(new ParallelCommandGroup( // run the following commands in parallel: + new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_TRENCH_FRONT_SPEED), + new TurretSetAbs((-40.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + // turn on the intake gently while shooting to help balls settle + addCommands(new IntakeSetRollers(-0.2)); + addCommands(new ShooterReadyAimFire(5.0)); + + // turn the shooter wheel and intake off now that the shooting is all done + addCommands(new ShooterWheelSet(0.0)); + addCommands(new IntakeSetRollers(0.0)); + + // turn the wheel off now that the shooting is all done + addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java new file mode 100644 index 0000000..29d619b --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java @@ -0,0 +1,42 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWShoot3 extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWShoot3() { + + addCommands(new DriveZeroGyro(0.0)); + + // first, lower the intake, start the shooter wheel, and wait until the turret + // is turned towards the target + addCommands(new ParallelCommandGroup( // run the following commands in parallel: + new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), + new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION), + new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + addCommands(new ShooterReadyAimFire(1.0)); + + // turn the shooter wheel and intake off now that the shooting is all done + addCommands(new ParallelCommandGroup( // below commands in parallel + new ShooterWheelSet(0.0), // + new IntakeSetRollers(0.0), // turn off the rollers + new HoodSetAbs(Hood.HOOD_STARTING_POSITION))); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java new file mode 100644 index 0000000..ac8b4dc --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWShoot3ThenToRP extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWShoot3ThenToRP() { + + // start backwards and shoot the first three balls + addCommands(new StartFWShoot3()); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToWall.java new file mode 100644 index 0000000..8b3d637 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToWall.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWShoot3ThenToWall extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWShoot3ThenToWall() { + + // start backwards and shoot the first three balls + addCommands(new StartFWShoot3()); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 40, 0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ChimneySet.java b/src/main/java/org/mayheminc/robot2020/commands/ChimneySet.java new file mode 100644 index 0000000..36f2e61 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ChimneySet.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ChimneySet extends CommandBase { + double m_speed; + + /** + * Creates a new ChimneySet. + */ + public ChimneySet(double d) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.chimney); + m_speed = d; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.chimney.setChimneySpeed(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.chimney.setChimneySpeed(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java new file mode 100644 index 0000000..195f07c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class HoodSetAbsWhileHeld extends InstantCommand { + double m_set; + + /** + * Creates a new HoodSetAbs. + */ + public HoodSetAbsWhileHeld(double set) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + + m_set = set; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.setPosition(m_set); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollersWhileHeld.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollersWhileHeld.java new file mode 100644 index 0000000..0ad8098 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollersWhileHeld.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeSetRollersWhileHeld extends CommandBase { + double m_speed; + + /** + * Creates a new IntakeSetRollers. + */ + public IntakeSetRollersWhileHeld(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + m_speed = speed; + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.intake.setRollers(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.intake.setRollers(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/RevolverDefault.java b/src/main/java/org/mayheminc/robot2020/commands/RevolverDefault.java new file mode 100644 index 0000000..495169c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/RevolverDefault.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class RevolverDefault extends CommandBase { + /** + * Creates a new RevolverDefault. + */ + public RevolverDefault() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.revolver); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + RobotContainer.revolver.setRevolverSpeed(-RobotContainer.OPERATOR_PAD.getLeftYAxis()); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/RevolverSetTurntable.java b/src/main/java/org/mayheminc/robot2020/commands/RevolverSetTurntable.java new file mode 100644 index 0000000..2dab623 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/RevolverSetTurntable.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class RevolverSetTurntable extends CommandBase { + double m_speed; + + /** + * Creates a new RevolverSetTurntable. + */ + public RevolverSetTurntable(double d) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.revolver); + m_speed = d; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.revolver.setRevolverSpeed(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.revolver.setRevolverSpeed(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java new file mode 100644 index 0000000..613351d --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterPermissionToFire extends CommandBase { + + /** + * Creates a new ShooterPositionToFire This command is intended to shoot while + * the driver holds the "Permission To Fire" button + */ + public ShooterPermissionToFire() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.chimney); + addRequirements(RobotContainer.feeder); + addRequirements(RobotContainer.hood); + addRequirements(RobotContainer.revolver); + addRequirements(RobotContainer.shooterWheel); + addRequirements(RobotContainer.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // do the following: command the shooterWheel to turn at speed, aimTurret, + // aimHood + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // if shooterWheels at speed, turret aimed, and hood aimed, then fire! + // firing actions as follows: + // feed roller after 0.0 seconds since start of firing + // chimney after 0.1 seconds since start of firing + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java b/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java new file mode 100644 index 0000000..a19fceb --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.util.MayhemTalonSRX; + +public class Revolver extends SubsystemBase { + private final MayhemTalonSRX revolverTalon = new MayhemTalonSRX(Constants.Talon.REVOLVER_TURNTABLE); + + /** + * Creates a new Revolver. + */ + public Revolver() { + ConfigureTalon(revolverTalon); + revolverTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + } + + private void ConfigureTalon(MayhemTalonSRX talon) { + talon.setNeutralMode(NeutralMode.Coast); + talon.configNominalOutputVoltage(+0.0f, -0.0f); + talon.configPeakOutputVoltage(+12.0, -12.0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + updateSmartDashboard(); + } + + void updateSmartDashboard() { + SmartDashboard.putNumber("Revolver Speed", revolverTalon.getSpeed()); + } + + + public void setRevolverSpeed(double speed) { + revolverTalon.set(ControlMode.PercentOutput, speed); + } + +} From 1e3550db6a9f2c4b377bb06fecf6fd4b6163d2f8 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 27 Feb 2020 13:50:53 -0500 Subject: [PATCH 080/121] Changes from late night Wednesday before GSD. Mostly related to targeting and initial curve-fits for shooter range determination, wheel speed, and hood angle. --- .../mayheminc/robot2020/RobotContainer.java | 13 ++-- .../autonomousroutines/StartBWOppTrench.java | 6 +- .../autonomousroutines/StartBWShoot3.java | 4 +- .../autonomousroutines/StartBWTrench.java | 6 +- .../autonomousroutines/StartFWRendezvous.java | 7 +-- .../autonomousroutines/StartFWShoot3.java | 4 +- .../commands/ShooterAimToTarget.java | 56 +++++++++++++++++ .../commands/ShooterPermissionToFire.java | 17 ++++- .../commands/ShooterReadyAimFire.java | 2 +- .../mayheminc/robot2020/subsystems/Hood.java | 17 ++--- .../robot2020/subsystems/Intake.java | 9 ++- .../robot2020/subsystems/Targeting.java | 62 +++++++++++++++++-- 12 files changed, 168 insertions(+), 35 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 2fa2fdf..06df4c9 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -177,8 +177,8 @@ private void configureDriverPadButtons() { // about -30 degrees // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new // ShooterSetTurretVBus(+0.2));// about +30 degrees - DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION)); - DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); + DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION)); + DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new // ShooterSetTurretRel(-200.0)); @@ -195,8 +195,8 @@ private void configureDriverPadButtons() { // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); // Debug shooter pid velocity - DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(100.0)); - DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-100.0)); + DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(50.0)); + DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-50.0)); DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0)); DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(3000)); @@ -204,7 +204,8 @@ private void configureDriverPadButtons() { // TODO: above hard-coded constant (3000) should be a named constant from // Shooter.java - DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterPermissionToFire()); + DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0)); } @@ -221,7 +222,7 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySet(1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollersWhileHeld(-1.0)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whileHeld(new TurretAimToTarget()); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whenPressed(new ShooterAimToTarget()); OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollersWhileHeld(1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(true)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java index 863ad54..ad857f7 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java @@ -29,7 +29,7 @@ public StartBWOppTrench() { addCommands(new IntakeSetRollers(-1.0)); // make sure the hood is down - addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); + addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); // drive to get balls from opponent's trench addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 108, 180)); @@ -46,7 +46,7 @@ public StartBWOppTrench() { // in shooting position, prepare everything for shooting addCommands(new ParallelCommandGroup( // run the following commands in parallel: new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), - new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION + 3000.0), + new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION + 3000.0), new TurretSetAbs((105.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); // turn on the intake gently while shooting to help balls settle @@ -58,6 +58,6 @@ public StartBWOppTrench() { addCommands(new IntakeSetRollers(0.0)); // turn the wheel off now that the shooting is all done - addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); + addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java index 2e5a3a1..dc5dbdc 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java @@ -28,7 +28,7 @@ public StartBWShoot3() { addCommands(new ParallelCommandGroup( // run the following commands in parallel: new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), - new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION), + new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); addCommands(new ShooterReadyAimFire(1.5)); @@ -37,6 +37,6 @@ public StartBWShoot3() { addCommands(new ParallelCommandGroup( // below commands in parallel new ShooterWheelSet(0.0), // new IntakeSetRollers(0.0), // turn off the rollers - new HoodSetAbs(Hood.HOOD_STARTING_POSITION))); + new HoodSetAbs(Hood.STARTING_POSITION))); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java index f91eb08..06b44d5 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java @@ -32,7 +32,7 @@ public StartBWTrench() { // intake while driving down the trench new IntakeSetRollers(-1.0), // ensure the hood is down - new HoodSetAbsWhileHeld(Hood.HOOD_STARTING_POSITION), + new HoodSetAbsWhileHeld(Hood.STARTING_POSITION), // drive the path under the control panel to the end new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 180, 180))); @@ -50,7 +50,7 @@ public StartBWTrench() { // wheels, raising the hood, and re-aiming the turret addCommands(new ParallelCommandGroup( new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), - new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION), + new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)), new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160))); @@ -68,6 +68,6 @@ public StartBWTrench() { addCommands(new IntakeSetRollers(0.0)); // turn the wheel off now that the shooting is all done - addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); + addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java index 84bd34c..d548b59 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java @@ -31,7 +31,7 @@ public StartFWRendezvous() { addCommands(new ParallelCommandGroup( // run the following commands in parallel: new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), - new HoodSetAbs(Hood.HOOD_INITIATION_LINE_POSITION), + new HoodSetAbs(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); addCommands(new ShooterReadyAimFire(1.0)); @@ -44,8 +44,7 @@ public StartFWRendezvous() { addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); // rais the hood a little to shoot from this increased distance - addCommands(new HoodSetAbsWhileHeld( - (Hood.HOOD_INITIATION_LINE_POSITION + Hood.HOOD_TRENCH_MID_POSITION) / 2.0)); + addCommands(new HoodSetAbsWhileHeld((Hood.INITIATION_LINE_POSITION + Hood.TRENCH_MID_POSITION) / 2.0)); addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 144, 50)); // turn on the intake to pick up balls @@ -70,6 +69,6 @@ public StartFWRendezvous() { addCommands(new IntakeSetRollers(0.0)); // turn the wheel off now that the shooting is all done - addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_TARGET_ZONE_POSITION)); + addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java index 29d619b..b065609 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java @@ -28,7 +28,7 @@ public StartFWShoot3() { addCommands(new ParallelCommandGroup( // run the following commands in parallel: new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), - new HoodSetAbsWhileHeld(Hood.HOOD_INITIATION_LINE_POSITION), + new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); addCommands(new ShooterReadyAimFire(1.0)); @@ -37,6 +37,6 @@ public StartFWShoot3() { addCommands(new ParallelCommandGroup( // below commands in parallel new ShooterWheelSet(0.0), // new IntakeSetRollers(0.0), // turn off the rollers - new HoodSetAbs(Hood.HOOD_STARTING_POSITION))); + new HoodSetAbs(Hood.STARTING_POSITION))); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java new file mode 100644 index 0000000..48b84ce --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterAimToTarget extends CommandBase { + /** + * Creates a new TurretAimToTarget. + */ + public ShooterAimToTarget() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + addRequirements(RobotContainer.shooterWheel); + addRequirements(RobotContainer.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // aim the hood and turret + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); + RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // aim the hood and turret + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); + RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (!interrupted) { + RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getDesiredWheelSpeed()); + + // want to also schedule a command here that does the shooting! + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (RobotContainer.hood.isAtPosition() && RobotContainer.turret.isAtDesiredPosition()); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java index 613351d..745db5f 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java @@ -30,17 +30,30 @@ public ShooterPermissionToFire() { // Called when the command is initially scheduled. @Override public void initialize() { - // do the following: command the shooterWheel to turn at speed, aimTurret, - // aimHood + // command the shooterWheel to turn at speed, aimHood, aimTurret + // RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getDesiredWheelSpeed()); + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); + RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth()); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + // command the shooterWheel to turn at speed + // RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getWheelSpeedFromY()); + + // aim the hood + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); + + // aim the turret + RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth()); + + // if speed is good, turret is aimed, and hood is aimed, start shooting! // if shooterWheels at speed, turret aimed, and hood aimed, then fire! // firing actions as follows: // feed roller after 0.0 seconds since start of firing // chimney after 0.1 seconds since start of firing + } // Returns true when the command should end. diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index a87c5b9..172f94f 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -46,6 +46,6 @@ public ShooterReadyAimFire(double waitDuration) { new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0), new Wait(0.1))); // Lower the hood now that we're done shooting - addCommands(new HoodSetAbsWhileHeld(Hood.HOOD_STARTING_POSITION)); + addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java index f276ea4..5f3f2ce 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java @@ -14,13 +14,15 @@ public class Hood extends SubsystemBase implements PidTunerObject { private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); - public final static double HOOD_STARTING_POSITION = 0; - public final static double HOOD_TARGET_ZONE_POSITION = 5000; - public final static double HOOD_INITIATION_LINE_POSITION = 65000; - public final static double HOOD_TRENCH_MID_POSITION = 80000; - + private final static int MIN_POSITION = 0; + private final static int MAX_POSITION = 90000; private final static double POSITION_TOLERANCE = 1000.0; + public final static double STARTING_POSITION = 0; + public final static double TARGET_ZONE_POSITION = 5000; + public final static double INITIATION_LINE_POSITION = 65000; + public final static double TRENCH_MID_POSITION = 80000; + private double m_desiredPosition = 0.0; /** @@ -49,9 +51,9 @@ private void configureTalon() { hoodTalon.setInverted(true); hoodTalon.setSensorPhase(true); - hoodTalon.configForwardSoftLimitThreshold(100000); + hoodTalon.configForwardSoftLimitThreshold(MAX_POSITION); hoodTalon.configForwardSoftLimitEnable(true); - hoodTalon.configReverseSoftLimitThreshold(0); + hoodTalon.configReverseSoftLimitThreshold(MIN_POSITION); hoodTalon.configReverseSoftLimitEnable(true); } @@ -63,6 +65,7 @@ public void periodic() { private void UpdateDashboard() { SmartDashboard.putNumber("Shooter Hood Pos", hoodTalon.getPosition()); + SmartDashboard.putNumber("Shooter Hood Setpoint", m_desiredPosition); } public void zero() { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index e1f2213..a5cbfdc 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -124,7 +124,14 @@ private void updatePivotPower() { // if the pivot is close enough, turn the motor on gently downards if (Math.abs(pivotTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) { isMoving = false; - setPivotVBus(-0.05); + + // if the current position is closer to PIVOT UP than PIVOT DOWN, apply a little + // positive power. + if (pivotTalon.getPosition() > PIVOT_UP / 2) { + setPivotVBus(+0.05); + } else { // we are close to the PIVOT DOWN, so apply a little negative power. + setPivotVBus(-0.05); + } } else { pivotTalon.set(ControlMode.Position, m_targetPosition, DemandType.ArbitraryFeedForward, m_feedForward); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index ea90715..daf4be1 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -27,11 +27,11 @@ public class Targeting extends SubsystemBase { // Diagonal FOV = 78.0 // Horizontal FOV = 70.42 // Vertical FOV = 43.3 - // NOTE: 76.5 horizontal FOV determined empirically by Ken on 2/22/2020 + // NOTE: 76.5 horizontal FOV determined empirically by Ken on 2/22/2020 private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 76.5; // Define the "target location" to be halfway from left to right - private final double CENTER_OF_TARGET_X = 0.510; + private final double CENTER_OF_TARGET_X = 0.475; // Calculate ticks per degree. // encoder ticks * turret pulley teeth / drive pulley teeth / 360 degrees @@ -45,6 +45,8 @@ public class Targeting extends SubsystemBase { private static final double AZIMUTH_CORRECTION_OFFSET = 0.0; // was -2.0 at CMP private double m_desiredAzimuth; + private double m_desiredHood; + private double m_desiredWheelSpeed; private double[] m_target_array; private int m_priorFrameCount; private double m_priorFrameTime; @@ -108,14 +110,14 @@ public void update() { m_bestY = 0.0; m_tilt = 0.0; m_area = 0.0; - m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); + // m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); } else if (m_target_array[0] < 0.0) { // this means the array has no valid data. Set m_xError = 0.0 m_bestX = 0.0; m_bestY = 0.0; m_tilt = 0.0; m_area = 0.0; - m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); + // m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); } else { // We have a valid data array. // There are three different situations: @@ -131,6 +133,8 @@ public void update() { m_area = m_target_array[3]; m_desiredAzimuth = findDesiredAzimuth(m_bestX, m_bestY, m_tilt, m_area); + m_desiredHood = getHoodFromY(); + m_desiredWheelSpeed = getWheelSpeedFromY(); } // at this point in the code, the "selected" target should be in the "best" @@ -138,12 +142,26 @@ public void update() { SmartDashboard.putNumber("m_bestY", m_bestY); SmartDashboard.putNumber("m_tilt", m_tilt); SmartDashboard.putNumber("m_area", m_area); + + SmartDashboard.putNumber("Range per Y", this.getRangeFromY()); + SmartDashboard.putNumber("Range per Area", this.getRangeFromArea()); + + SmartDashboard.putNumber("Wheel Speed From Y", this.getWheelSpeedFromY()); + SmartDashboard.putNumber("Hood Angle From Y", this.getHoodFromY()); } public double getDesiredAzimuth() { return m_desiredAzimuth + AZIMUTH_CORRECTION_OFFSET; } + public double getDesiredHood() { + return m_desiredHood; + } + + public double getDesiredWheelSpeed() { + return m_desiredWheelSpeed; + } + public double getRecommendedSpeed() { // Returns a speed based upon the Y value double speed; @@ -199,4 +217,40 @@ public double findDesiredAzimuth(double X, double Y, double tilt, double area) { return desiredAzimuth; } + /** + * use m_bestY to get the desired hood setting for the target + * + * @return + */ + private double getHoodFromY() { + return 105874 + -407324 * m_bestY + 881911 * m_bestY * m_bestY + -506286 * m_bestY * m_bestY * m_bestY; + } + + /** + * use m_bestY to get the desired wheel speed for the target + * + * @return + */ + private double getWheelSpeedFromY() { + return 2618 + -1939 * m_bestY + 3583 * m_bestY * m_bestY; + } + + /** + * use m_bestY to get the range in feet to the target. + * + * @return + */ + public double getRangeFromY() { + return 8.11 + -9.17 * m_bestY + 33.9 * m_bestY * m_bestY; + } + + /** + * Use the area to calculate the range in feet. + * + * @return + */ + public double getRangeFromArea() { + return 0.912 * Math.pow(m_area, -0.695); + } + } From 17a749b1d3908f857dcc1b924168c65f0c701ecf Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 27 Feb 2020 23:46:35 -0500 Subject: [PATCH 081/121] Changes from at the practice field on the morning of GSD load-in day, as well as changes made at the event on Thursday night: new "1-button" ShooterFiringSequency, enhancements and code-sharing of some autonomous programs, --- .../mayheminc/robot2020/RobotContainer.java | 29 ++++++----- .../autonomousroutines/StartBWOppTrench.java | 5 +- .../autonomousroutines/StartBWShoot3.java | 12 +++-- .../autonomousroutines/StartBWTrench.java | 11 +++-- .../autonomousroutines/StartBWTrench3.java | 19 +++++++ .../autonomousroutines/StartBWTrench5.java | 19 +++++++ .../autonomousroutines/StartFWRendezvous.java | 18 ++++--- .../commands/HoodSetAbsWhileHeld.java | 9 +++- .../PrintAutonomousTimeRemaining.java | 9 ++-- .../commands/ShooterAimToTarget.java | 3 +- .../robot2020/commands/ShooterCeaseFire.java | 35 +++++++++++++ .../commands/ShooterFiringSequence.java | 43 ++++++++++++++++ .../commands/ShooterWheelSetToTarget.java | 49 +++++++++++++++++++ .../robot2020/subsystems/Intake.java | 11 ++++- .../robot2020/subsystems/Shooter.java | 0 .../robot2020/subsystems/ShooterWheel.java | 28 +++++++---- .../robot2020/subsystems/Targeting.java | 30 +----------- 17 files changed, 253 insertions(+), 77 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench3.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTarget.java delete mode 100644 src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 06df4c9..d57d59d 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -106,18 +106,19 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - autonomousPrograms.push(/* 11 */ new StayStill()); - autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToRP()); - autonomousPrograms.push(/* 09 */ new StartBWDriveOnlyToWall()); - autonomousPrograms.push(/* 08 */ new StartFWDriveOnlyToRP()); - autonomousPrograms.push(/* 07 */ new StartFWDriveOnlyToWall()); - autonomousPrograms.push(/* 06 */ new StartBWShoot3ThenToRP()); - autonomousPrograms.push(/* 05 */ new StartBWShoot3ThenToWall()); - autonomousPrograms.push(/* 04 */ new StartFWShoot3ThenToRP()); - autonomousPrograms.push(/* 03 */ new StartFWShoot3ThenToWall()); - autonomousPrograms.push(/* 02 */ new StartFWRendezvous()); - autonomousPrograms.push(/* 01 */ new StartBWOppTrench()); - autonomousPrograms.push(/* 00 */ new StartBWTrench()); + autonomousPrograms.push(/* 12 */ new StayStill()); + autonomousPrograms.push(/* 11 */ new StartBWDriveOnlyToRP()); + autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToWall()); + autonomousPrograms.push(/* 09 */ new StartFWDriveOnlyToRP()); + autonomousPrograms.push(/* 08 */ new StartFWDriveOnlyToWall()); + autonomousPrograms.push(/* 07 */ new StartBWShoot3ThenToRP()); + autonomousPrograms.push(/* 06 */ new StartBWShoot3ThenToWall()); + autonomousPrograms.push(/* 05 */ new StartFWShoot3ThenToRP()); + autonomousPrograms.push(/* 04 */ new StartFWShoot3ThenToWall()); + autonomousPrograms.push(/* 03 */ new StartFWRendezvous()); + autonomousPrograms.push(/* 02 */ new StartBWOppTrench()); + autonomousPrograms.push(/* 01 */ new StartBWTrench3()); + autonomousPrograms.push(/* 00 */ new StartBWTrench5()); autonomous.setAutonomousPrograms(autonomousPrograms); @@ -204,7 +205,9 @@ private void configureDriverPadButtons() { // TODO: above hard-coded constant (3000) should be a named constant from // Shooter.java - DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whileHeld(new ShooterPermissionToFire()); + DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); + DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0)); } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java index ad857f7..656c7f3 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java @@ -27,6 +27,7 @@ public StartBWOppTrench() { // lower the intake and turn it on before driving forwards addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); addCommands(new IntakeSetRollers(-1.0)); + addCommands(new Wait(5.0)); // make sure the hood is down addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); @@ -51,7 +52,9 @@ public StartBWOppTrench() { // turn on the intake gently while shooting to help balls settle addCommands(new IntakeSetRollers(-0.2)); - addCommands(new ShooterReadyAimFire(5.0)); + + // use the "one button" firing sequence + addCommands(new ShooterFiringSequence(5.0)); // turn the shooter wheel and intake off now that the shooting is all done addCommands(new ShooterWheelSet(0.0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java index dc5dbdc..9619bdb 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java @@ -31,12 +31,14 @@ public StartBWShoot3() { new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); - addCommands(new ShooterReadyAimFire(1.5)); + addCommands(new ShooterFiringSequence(1.5)); + + // note that the above turns everything off again when it is done. // turn the shooter wheel and intake off now that the shooting is all done - addCommands(new ParallelCommandGroup( // below commands in parallel - new ShooterWheelSet(0.0), // - new IntakeSetRollers(0.0), // turn off the rollers - new HoodSetAbs(Hood.STARTING_POSITION))); + // addCommands(new ParallelRaceGroup( // below commands in parallel + // new ShooterWheelSet(0.0), // + // new IntakeSetRollers(0.0), // turn off the rollers + // new HoodSetAbs(Hood.STARTING_POSITION))); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java index 06b44d5..f02e622 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java @@ -19,13 +19,14 @@ public class StartBWTrench extends SequentialCommandGroup { /** * Add your docs here. */ - public StartBWTrench() { + public StartBWTrench(double extraDistance) { + // Note: extra distance planned to be 40 inches // start backwards and shoot the first three balls addCommands(new StartBWShoot3()); // then, drive to the trench entrance (jog left a little to get there) - addCommands(new DriveStraightOnHeading(0.5, DistanceUnits.INCHES, 40, 150)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 46, 140)); // pick up balls while heading down the trench. addCommands(new ParallelCommandGroup( @@ -34,7 +35,7 @@ public StartBWTrench() { // ensure the hood is down new HoodSetAbsWhileHeld(Hood.STARTING_POSITION), // drive the path under the control panel to the end - new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 180, 180))); + new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 140 + extraDistance, 180))); // now driven to the balls at far end of trench addCommands(new Wait(0.8), // wait for last two balls to get into robot @@ -44,7 +45,7 @@ public StartBWTrench() { // after getting all three balls, go back to shooting position // first, make sure we drive straight out from under the control panel - addCommands(new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 48, 180)); + addCommands(new DriveStraightOnHeading(-0.6, DistanceUnits.INCHES, 8 + extraDistance, 180)); // drive diagonally over towards the shooting position, while turning on shooter // wheels, raising the hood, and re-aiming the turret @@ -61,7 +62,7 @@ public StartBWTrench() { addCommands(new IntakeSetRollers(-0.2)); // start the shooting sequence - addCommands(new ShooterReadyAimFire(6.0)); + addCommands(new ShooterFiringSequence(6.0)); // turn the shooter wheel and intake off now that the shooting is all done addCommands(new ShooterWheelSet(0.0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench3.java new file mode 100644 index 0000000..3dbb108 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench3.java @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWTrench3 extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWTrench3() { + addCommands(new StartBWTrench(0.0)); // use extraDistance of 0 inches to stop before control panel + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java new file mode 100644 index 0000000..9939116 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWTrench5 extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWTrench5() { + addCommands(new StartBWTrench(40)); // use extraDistance of 40 inches for control panel + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java index d548b59..a3f63a3 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java @@ -25,6 +25,8 @@ public StartFWRendezvous() { // start out facing in the normal direction addCommands(new DriveZeroGyro(0.0)); + addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + // shoot the 3 balls we started with // first, lower the intake, start the shooter wheel, and wait until the turret // is turned towards the target @@ -34,7 +36,7 @@ public StartFWRendezvous() { new HoodSetAbs(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); - addCommands(new ShooterReadyAimFire(1.0)); + addCommands(new ShooterFiringSequence(1.5)); // drive a little bit out to get away from other robots. addCommands(new DriveStraightOnHeading(-0.2, DistanceUnits.INCHES, 36, 30)); @@ -43,13 +45,17 @@ public StartFWRendezvous() { // further addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - // rais the hood a little to shoot from this increased distance + // raise the hood a little to shoot from this increased distance addCommands(new HoodSetAbsWhileHeld((Hood.INITIATION_LINE_POSITION + Hood.TRENCH_MID_POSITION) / 2.0)); - addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 144, 50)); + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 124, 40)); // turn on the intake to pick up balls addCommands(new IntakeSetRollers(-1.0)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 84, 30)); + addCommands(new DriveStraightOnHeading(0.15, DistanceUnits.INCHES, 72, 65)); + + // back up to get free of the boundary post, then turn and drive towards goal + addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 16, 20)); + addCommands(new DriveStraightOnHeading(+0.2, DistanceUnits.INCHES, 48, 20)); // now driven to get the balls, stay here and shoot them addCommands(new Wait(0.8), // wait for the balls to get into robot @@ -58,11 +64,11 @@ public StartFWRendezvous() { // in shooting position, prepare everything for shooting addCommands(new ParallelCommandGroup( // run the following commands in parallel: new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_TRENCH_FRONT_SPEED), - new TurretSetAbs((-40.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); // turn on the intake gently while shooting to help balls settle addCommands(new IntakeSetRollers(-0.2)); - addCommands(new ShooterReadyAimFire(5.0)); + addCommands(new ShooterFiringSequence(6.0)); // turn the shooter wheel and intake off now that the shooting is all done addCommands(new ShooterWheelSet(0.0)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java index 195f07c..505cf0a 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java @@ -9,9 +9,9 @@ import org.mayheminc.robot2020.RobotContainer; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.CommandBase; -public class HoodSetAbsWhileHeld extends InstantCommand { +public class HoodSetAbsWhileHeld extends CommandBase { double m_set; /** @@ -30,4 +30,9 @@ public void initialize() { RobotContainer.hood.setPosition(m_set); } + @Override + public boolean isFinished() { + return true; + } + } diff --git a/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java index ed73227..f65cd39 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java +++ b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java @@ -8,15 +8,18 @@ * */ public class PrintAutonomousTimeRemaining extends CommandBase { - String Message = ""; + String Message = ""; + public PrintAutonomousTimeRemaining(String msg) { this.Message = msg; } - + // Called just before this Command runs the first time @Override public void initialize() { - // DriverStation.reportError(Message + " At: " + Robot.autonomousTimeRemaining() + "\n", false); + + // DriverStation.reportError(Message + " At: " + Robot.autonomousTimeRemaining() + // + "\n", false); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java index 48b84ce..0069b50 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java @@ -18,7 +18,6 @@ public class ShooterAimToTarget extends CommandBase { public ShooterAimToTarget() { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.hood); - addRequirements(RobotContainer.shooterWheel); addRequirements(RobotContainer.turret); } @@ -42,7 +41,7 @@ public void execute() { @Override public void end(boolean interrupted) { if (!interrupted) { - RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getDesiredWheelSpeed()); + // RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getDesiredWheelSpeed()); // want to also schedule a command here that does the shooting! } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java new file mode 100644 index 0000000..5b72bb4 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Hood; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShooterCeaseFire extends SequentialCommandGroup { + /** + * Creates a new ShooterReadyAimFire. + */ + public ShooterCeaseFire() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + // turn off the subsystems that were used in shooting + // (feed roller, chimney, revolver, and shooter wheels) + + addCommands(new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0), + new ShooterWheelSetVBus(0.0), new Wait(0.1))); + + // Lower the hood now that we're done shooting + addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java new file mode 100644 index 0000000..66ce31f --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShooterFiringSequence extends SequentialCommandGroup { + /** + * Creates a new ShooterReadyAimFire. + */ + public ShooterFiringSequence(double waitDuration) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + // Turn off compressor while actively shooting. + addCommands(new AirCompressorPause()); + + // one last aim at the target before starting shooting. + addCommands(new ShooterAimToTarget()); + + addCommands(new ParallelRaceGroup(new ShooterWheelSetToTarget(true), new TurretAimToTarget())); + + // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the + // revolver turntable, shoot for specified duration + // TODO: should really shoot until no balls detected any more + addCommands( + new ParallelRaceGroup(new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), new Wait(waitDuration))); + + // turn off the feeder, chimney, and revolver, ending after 0.1 seconds + addCommands(new ShooterCeaseFire()); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTarget.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTarget.java new file mode 100644 index 0000000..7e2d2c8 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTarget.java @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterWheelSetToTarget extends CommandBase { + double m_rpm; + boolean m_waitForDone; + + public ShooterWheelSetToTarget() { + this(false); + } + + /** + * Creates a new ShooterWheelSet + */ + public ShooterWheelSetToTarget(boolean wait) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_rpm = 3000; // default setting + m_waitForDone = wait; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_rpm = RobotContainer.targeting.getDesiredWheelSpeed(); + RobotContainer.shooterWheel.setSpeed(m_rpm); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_waitForDone) { + return (Math.abs(m_rpm - RobotContainer.shooterWheel.getSpeed()) < 100); + } else { + return true; + } + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index a5cbfdc..c0d1ad4 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -17,6 +17,7 @@ import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.PidTunerObject; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -30,6 +31,7 @@ public class Intake extends SubsystemBase implements PidTunerObject { public final double PIVOT_DOWN = 0.0; private static final double HORIZONTAL_HOLD_OUTPUT = 0.00; + private static final double MAX_PID_MOVEMENT_TIME_SEC = 10.0; enum PivotMode { MANUAL_MODE, PID_MODE, @@ -39,6 +41,7 @@ enum PivotMode { boolean isMoving; double m_targetPosition; double m_feedForward; + Timer m_pidTimer = new Timer(); /** * Creates a new Intake. @@ -103,6 +106,8 @@ public void setPivot(Double b) { m_targetPosition = b; mode = PivotMode.PID_MODE; isMoving = true; + + m_pidTimer.start(); } public boolean isPivotAtPosition() { @@ -121,8 +126,10 @@ public void periodic() { private void updatePivotPower() { if (mode == PivotMode.PID_MODE) { - // if the pivot is close enough, turn the motor on gently downards - if (Math.abs(pivotTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) { + // if the pivot is close enough or it has been on too long, turn the motor on + // gently downards + if ((Math.abs(pivotTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) + || m_pidTimer.hasPeriodPassed(Intake.MAX_PID_MOVEMENT_TIME_SEC)) { isMoving = false; // if the current position is closer to PIVOT UP than PIVOT DOWN, apply a little diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java b/src/main/java/org/mayheminc/robot2020/subsystems/Shooter.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java index 292584c..db7c281 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -22,15 +22,16 @@ public class ShooterWheel extends SubsystemBase implements PidTunerObject { public static final double SHOOTER_WHEEL_INITIATION_LINE_SPEED = 3000.0; public static final double SHOOTER_WHEEL_TRENCH_FRONT_SPEED = 3400.0; + private static final double MAX_SPEED_RPM = 3500; double m_targetSpeedRPM; - // Note: for ease of thinking, 1 RPM =~ 3.4 native units for the shooter + // Note: for ease of thinking, 1 RPM =~ 3.4 native units for the shooter double convertRpmToTicksPer100ms(double rpm) { return rpm / SECONDS_PER_MINUTE * TALON_TICKS_PER_REV / HUNDRED_MS_PER_SECOND; } - // Note: 3.413 native units =~ 1.0 RPM for the shooter + // Note: 3.413 native units =~ 1.0 RPM for the shooter double convertTicksPer100msToRPM(double ticks) { return ticks * HUNDRED_MS_PER_SECOND / TALON_TICKS_PER_REV * SECONDS_PER_MINUTE; } @@ -46,6 +47,7 @@ public void init() { configureWheelFalcons(); setSpeedVBus(0.0); } + // configure a pair of shooter wheel falcons private void configureWheelFalcons() { // most of the configuration is shared for the two Falcons @@ -56,23 +58,25 @@ private void configureWheelFalcons() { shooterWheelLeft.setInverted(false); shooterWheelRight.setInverted(true); } + private void configureOneWheelFalcon(MayhemTalonSRX shooterWheelFalcon) { shooterWheelFalcon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); shooterWheelFalcon.setNeutralMode(NeutralMode.Coast); shooterWheelFalcon.configNominalOutputVoltage(+0.0f, -0.0f); shooterWheelFalcon.configPeakOutputVoltage(+12.0, 0.0); - shooterWheelFalcon.configNeutralDeadband(0.001); // Config neutral deadband to be the smallest possible + shooterWheelFalcon.configNeutralDeadband(0.001); // Config neutral deadband to be the smallest possible // For PIDF computations, 1023 is interpreted as "full" motor output // Velocity is in native units of TicksPer100ms. // 1000rpm =~ 3413 native units. - // P of "3.0" means that full power applied with error of 341 native units = 100rpm - // (above also means that 50% power boost applied with error of 50rpm) - shooterWheelFalcon.config_kP(0, 0.1, 0); // previously used 3.0 + // P of "3.0" means that full power applied with error of 341 native units = + // 100rpm + // (above also means that 50% power boost applied with error of 50rpm) + shooterWheelFalcon.config_kP(0, 0.1, 0); // previously used 3.0 shooterWheelFalcon.config_kI(0, 0.0, 0); - shooterWheelFalcon.config_kD(0, 0.0, 0); // CTRE recommends starting at 10x P-gain - shooterWheelFalcon.config_kF(0, 0.046, 0); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); - shooterWheelFalcon.configAllowableClosedloopError(0, 0, 0); // no "neutral" zone around target + shooterWheelFalcon.config_kD(0, 0.0, 0); // CTRE recommends starting at 10x P-gain + shooterWheelFalcon.config_kF(0, 0.046, 0); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); + shooterWheelFalcon.configAllowableClosedloopError(0, 0, 0); // no "neutral" zone around target } @Override @@ -81,7 +85,6 @@ public void periodic() { UpdateDashboard(); } - private void UpdateDashboard() { SmartDashboard.putNumber("Shooter Wheel pos", shooterWheelLeft.getSelectedSensorPosition(0)); SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelLeft.getSelectedSensorVelocity(0)); @@ -117,6 +120,11 @@ public void zero() { * @param rpm */ public void setSpeed(double rpm) { + + if (rpm > MAX_SPEED_RPM) { + rpm = MAX_SPEED_RPM; + } + m_targetSpeedRPM = rpm; double ticks = convertRpmToTicksPer100ms(rpm); shooterWheelLeft.set(ControlMode.Velocity, ticks); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index daf4be1..4d0cef4 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -37,9 +37,6 @@ public class Targeting extends SubsystemBase { // encoder ticks * turret pulley teeth / drive pulley teeth / 360 degrees private final double TICKS_PER_DEGREE = (4096.0 * 225.0 / 18.0 / 360.0); // was 6300 / 45 - private static final double SPEED_EQ_M = -4.115; - private static final double SPEED_EQ_B = 2.244; - // After computing a desired azimuth, add a "fudge" offset to correct // empirically measured error. Offset should be in azimuth "ticks." private static final double AZIMUTH_CORRECTION_OFFSET = 0.0; // was -2.0 at CMP @@ -162,29 +159,6 @@ public double getDesiredWheelSpeed() { return m_desiredWheelSpeed; } - public double getRecommendedSpeed() { - // Returns a speed based upon the Y value - double speed; - - if (m_bestY < 0.1) { - // can't see the target, set speed to something real slow - speed = 0.2; - } else { - - // use the equation to determine the speed from m_bestY - speed = (SPEED_EQ_M * m_bestY) + SPEED_EQ_B; - - // enforce min speed of 0.30 and max speed of 0.90 - if (speed < 0.30) { - speed = 0.30; - } else if (speed > 0.90) { - speed = 0.90; - } - } - - return speed; - } - /** * Return the desired turrent encoder ticks in the turret for the center of the * target. @@ -240,7 +214,7 @@ private double getWheelSpeedFromY() { * * @return */ - public double getRangeFromY() { + private double getRangeFromY() { return 8.11 + -9.17 * m_bestY + 33.9 * m_bestY * m_bestY; } @@ -249,7 +223,7 @@ public double getRangeFromY() { * * @return */ - public double getRangeFromArea() { + private double getRangeFromArea() { return 0.912 * Math.pow(m_area, -0.695); } From 7f1baeb492e37c13dc1bdbd368277ed0ee21d6a3 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Fri, 28 Feb 2020 17:12:31 -0500 Subject: [PATCH 082/121] Code as of half-way through first day of Granite State District Event. Minor tweaks to autonomous programs and ShooterFiringSequence. Minor naming convention tweaks. --- .../mayheminc/robot2020/RobotContainer.java | 5 ++--- .../autonomousroutines/StartBWOppTrench.java | 5 ++++- .../commands/ShooterFiringSequence.java | 9 +++++---- .../commands/ShooterReadyAimFire.java | 2 +- ...ava => TurretAimToTargetContinuously.java} | 8 ++++++-- .../robot2020/subsystems/Climber.java | 14 ++++++------- .../robot2020/subsystems/Intake.java | 20 +++++++++---------- 7 files changed, 35 insertions(+), 28 deletions(-) rename src/main/java/org/mayheminc/robot2020/commands/{TurretAimToTarget.java => TurretAimToTargetContinuously.java} (81%) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index d57d59d..a06fad8 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -199,11 +199,10 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(50.0)); DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-50.0)); DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0)); - DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(3000)); + DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON + .whenPressed(new ShooterWheelSet(RobotContainer.shooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED)); DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1)); - // TODO: above hard-coded constant (3000) should be a named constant from - // Shooter.java DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0)); DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java index 656c7f3..7048b03 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java @@ -27,7 +27,7 @@ public StartBWOppTrench() { // lower the intake and turn it on before driving forwards addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); addCommands(new IntakeSetRollers(-1.0)); - addCommands(new Wait(5.0)); + addCommands(new Wait(2.5)); // make sure the hood is down addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); @@ -53,6 +53,9 @@ public StartBWOppTrench() { // turn on the intake gently while shooting to help balls settle addCommands(new IntakeSetRollers(-0.2)); + // wait 2/10 of a second to get some camera data after turning turret + addCommands(new Wait(0.2)); + // use the "one button" firing sequence addCommands(new ShooterFiringSequence(5.0)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java index 66ce31f..2a365da 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java @@ -28,14 +28,15 @@ public ShooterFiringSequence(double waitDuration) { // one last aim at the target before starting shooting. addCommands(new ShooterAimToTarget()); - addCommands(new ParallelRaceGroup(new ShooterWheelSetToTarget(true), new TurretAimToTarget())); + addCommands(new ParallelRaceGroup(new ShooterWheelSetToTarget(true), new TurretAimToTargetContinuously())); // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the // revolver turntable, shoot for specified duration // TODO: should really shoot until no balls detected any more - addCommands( - new ParallelRaceGroup(new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), - new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), new Wait(waitDuration))); + addCommands(new ParallelRaceGroup( // + new TurretAimToTargetContinuously(), // continue aiming while shooting + new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), new Wait(waitDuration))); // turn off the feeder, chimney, and revolver, ending after 0.1 seconds addCommands(new ShooterCeaseFire()); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index 172f94f..ee279f9 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -32,7 +32,7 @@ public ShooterReadyAimFire(double waitDuration) { // to speed addCommands(new ParallelRaceGroup( new ParallelCommandGroup(/* new TargetingIsOnTarget(), */ new ShooterWheelSet(3000.0, true)), - new TurretAimToTarget())); + new TurretAimToTargetContinuously())); // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the // revolver turntable, shoot for specified duration diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java similarity index 81% rename from src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java rename to src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java index b1a3814..ad2b463 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTarget.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java @@ -11,11 +11,11 @@ import edu.wpi.first.wpilibj2.command.CommandBase; -public class TurretAimToTarget extends CommandBase { +public class TurretAimToTargetContinuously extends CommandBase { /** * Creates a new TurretAimToTarget. */ - public TurretAimToTarget() { + public TurretAimToTargetContinuously() { // Use addRequirements() here to declare subsystem dependencies. addRequirements(RobotContainer.turret); } @@ -28,7 +28,11 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + // TODO: may want to only aim at the target if recent valid target data from the + // camera double pos = RobotContainer.targeting.getDesiredAzimuth(); + // TODO: if the desired azimuth is "beyond the soft stop" should we turn the + // robot? RobotContainer.turret.setPositionAbs(pos); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 55f8941..5f3f939 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -23,8 +23,10 @@ public class Climber extends SubsystemBase { private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT); private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT); - // private final MayhemTalonSRX walkerLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); - // private final MayhemTalonSRX walkerRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); + // private final MayhemTalonSRX walkerLeft = new + // MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); + // private final MayhemTalonSRX walkerRight = new + // MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); private final Solenoid pistons = new Solenoid(Constants.Solenoid.CLIMBER_PISTONS); @@ -33,8 +35,6 @@ public class Climber extends SubsystemBase { */ public Climber() { - // TODO: Add position control to go all the way to the top, and nearly all the way in for climbing - winchLeft.setNeutralMode(NeutralMode.Brake); winchLeft.configNominalOutputVoltage(+0.0f, -0.0f); winchLeft.configPeakOutputVoltage(+12.0, -12.0); @@ -52,7 +52,7 @@ public Climber() { winchRight.configForwardSoftLimitEnable(true); winchRight.configReverseSoftLimitThreshold(MIN_HEIGHT_SOFT_LIMIT); winchRight.configReverseSoftLimitEnable(true); - + // walkerRight.setNeutralMode(NeutralMode.Brake); // walkerRight.configNominalOutputVoltage(+0.0f, -0.0f); // walkerRight.configPeakOutputVoltage(+12.0, -12.0); @@ -86,11 +86,11 @@ public void setWinchRightSpeed(double power) { } // public void setWalkerLeftSpeed(double power) { - // walkerLeft.set(ControlMode.Velocity, power); + // walkerLeft.set(ControlMode.Velocity, power); // } // public void setWalkerRightSpeed(double power) { - // walkerRight.set(ControlMode.Velocity, power); + // walkerRight.set(ControlMode.Velocity, power); // } public void setPistons(boolean b) { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index c0d1ad4..6a64789 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -37,8 +37,8 @@ enum PivotMode { MANUAL_MODE, PID_MODE, }; - PivotMode mode = PivotMode.MANUAL_MODE; - boolean isMoving; + PivotMode m_mode = PivotMode.MANUAL_MODE; + boolean m_isMoving; double m_targetPosition; double m_feedForward; Timer m_pidTimer = new Timer(); @@ -104,14 +104,14 @@ public void setRollers(double power) { public void setPivot(Double b) { m_targetPosition = b; - mode = PivotMode.PID_MODE; - isMoving = true; + m_mode = PivotMode.PID_MODE; + m_isMoving = true; m_pidTimer.start(); } public boolean isPivotAtPosition() { - return !isMoving; + return !m_isMoving; } @Override @@ -125,12 +125,12 @@ public void periodic() { private void updatePivotPower() { - if (mode == PivotMode.PID_MODE) { + if (m_mode == PivotMode.PID_MODE) { // if the pivot is close enough or it has been on too long, turn the motor on // gently downards if ((Math.abs(pivotTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) || m_pidTimer.hasPeriodPassed(Intake.MAX_PID_MOVEMENT_TIME_SEC)) { - isMoving = false; + m_isMoving = false; // if the current position is closer to PIVOT UP than PIVOT DOWN, apply a little // positive power. @@ -171,14 +171,14 @@ public void updateSmartDashBoard() { SmartDashboard.putNumber("Intake Target", m_targetPosition); SmartDashboard.putNumber("Intake FeedForward", m_feedForward); - SmartDashboard.putBoolean("Intake Is Moving", isMoving); - SmartDashboard.putBoolean("Intake PID Mode", (mode == PivotMode.PID_MODE)); + SmartDashboard.putBoolean("Intake Is Moving", m_isMoving); + SmartDashboard.putBoolean("Intake PID Mode", (m_mode == PivotMode.PID_MODE)); SmartDashboard.putNumber("Intake Rollers", rollerTalon.getMotorOutputPercent()); } public void setPivotVBus(double VBus) { pivotTalon.set(ControlMode.PercentOutput, VBus); - mode = PivotMode.MANUAL_MODE; + m_mode = PivotMode.MANUAL_MODE; } @Override From c25fa83d2cb4bf2c39ddb41c66e714e36e7da485 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Fri, 28 Feb 2020 22:01:22 -0500 Subject: [PATCH 083/121] Changed shooter wheel to go to "idle speed" (1500rpm) after shooting, instead of turning it off completely (to prevent ball jams from a "stuck" ball in the shooter wheel.) Increased revolver speed for "fast" OI button to full speed (previously half speed), and removed a "dead" file (ShootAndDriveForward.java). --- .../mayheminc/robot2020/RobotContainer.java | 5 ++-- .../ShootAndDriveForward.java | 29 ------------------- .../autonomousroutines/StartBWOppTrench.java | 4 +-- .../autonomousroutines/StartBWShoot3.java | 8 +---- .../autonomousroutines/StartBWTrench.java | 5 ++-- .../autonomousroutines/StartFWRendezvous.java | 6 ++-- .../autonomousroutines/StartFWShoot3.java | 4 +-- .../robot2020/commands/ShooterCeaseFire.java | 4 ++- .../commands/ShooterFiringSequence.java | 15 ++++++---- .../robot2020/subsystems/ShooterWheel.java | 7 +++-- .../robot2020/subsystems/Targeting.java | 8 ++++- 11 files changed, 35 insertions(+), 60 deletions(-) delete mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index a06fad8..8a93143 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -199,8 +199,7 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(50.0)); DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-50.0)); DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0)); - DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON - .whenPressed(new ShooterWheelSet(RobotContainer.shooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED)); + DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED)); DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1)); @@ -217,7 +216,7 @@ private void configureOperatorStickButtons() { private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new RevolverSetTurntable(0.2)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(0.5)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(1.0)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); // new ShooterSetWheel(1000)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java deleted file mode 100644 index e2a0f26..0000000 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/ShootAndDriveForward.java +++ /dev/null @@ -1,29 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.mayheminc.robot2020.autonomousroutines; - -import org.mayheminc.robot2020.commands.ShooterWheelSet; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class ShootAndDriveForward extends SequentialCommandGroup { - /** - * Creates a new ShootAndDriveForward. - */ - public ShootAndDriveForward() { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - super(); - - addCommands(new ShooterWheelSet(5500)); - // addCommands(new Wait(2.0)); - } -} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java index 7048b03..dc3ae7d 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java @@ -46,7 +46,7 @@ public StartBWOppTrench() { // in shooting position, prepare everything for shooting addCommands(new ParallelCommandGroup( // run the following commands in parallel: - new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION + 3000.0), new TurretSetAbs((105.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); @@ -60,7 +60,7 @@ public StartBWOppTrench() { addCommands(new ShooterFiringSequence(5.0)); // turn the shooter wheel and intake off now that the shooting is all done - addCommands(new ShooterWheelSet(0.0)); + addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED)); addCommands(new IntakeSetRollers(0.0)); // turn the wheel off now that the shooting is all done diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java index 9619bdb..711ba47 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java @@ -27,18 +27,12 @@ public StartBWShoot3() { // is turned towards the target addCommands(new ParallelCommandGroup( // run the following commands in parallel: new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), - new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); addCommands(new ShooterFiringSequence(1.5)); // note that the above turns everything off again when it is done. - - // turn the shooter wheel and intake off now that the shooting is all done - // addCommands(new ParallelRaceGroup( // below commands in parallel - // new ShooterWheelSet(0.0), // - // new IntakeSetRollers(0.0), // turn off the rollers - // new HoodSetAbs(Hood.STARTING_POSITION))); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java index f02e622..08f065c 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java @@ -49,8 +49,7 @@ public StartBWTrench(double extraDistance) { // drive diagonally over towards the shooting position, while turning on shooter // wheels, raising the hood, and re-aiming the turret - addCommands(new ParallelCommandGroup( - new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + addCommands(new ParallelCommandGroup(new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)), new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160))); @@ -65,7 +64,7 @@ public StartBWTrench(double extraDistance) { addCommands(new ShooterFiringSequence(6.0)); // turn the shooter wheel and intake off now that the shooting is all done - addCommands(new ShooterWheelSet(0.0)); + addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED)); addCommands(new IntakeSetRollers(0.0)); // turn the wheel off now that the shooting is all done diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java index a3f63a3..7e87398 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java @@ -32,7 +32,7 @@ public StartFWRendezvous() { // is turned towards the target addCommands(new ParallelCommandGroup( // run the following commands in parallel: new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), - new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), new HoodSetAbs(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); @@ -63,7 +63,7 @@ public StartFWRendezvous() { // in shooting position, prepare everything for shooting addCommands(new ParallelCommandGroup( // run the following commands in parallel: - new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_TRENCH_FRONT_SPEED), + new ShooterWheelSet(ShooterWheel.TRENCH_FRONT_SPEED), new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); // turn on the intake gently while shooting to help balls settle @@ -71,7 +71,7 @@ public StartFWRendezvous() { addCommands(new ShooterFiringSequence(6.0)); // turn the shooter wheel and intake off now that the shooting is all done - addCommands(new ShooterWheelSet(0.0)); + addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED)); addCommands(new IntakeSetRollers(0.0)); // turn the wheel off now that the shooting is all done diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java index b065609..d5d16a9 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java @@ -27,7 +27,7 @@ public StartFWShoot3() { // is turned towards the target addCommands(new ParallelCommandGroup( // run the following commands in parallel: new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), - new ShooterWheelSet(ShooterWheel.SHOOTER_WHEEL_INITIATION_LINE_SPEED), + new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); @@ -35,7 +35,7 @@ public StartFWShoot3() { // turn the shooter wheel and intake off now that the shooting is all done addCommands(new ParallelCommandGroup( // below commands in parallel - new ShooterWheelSet(0.0), // + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), // new IntakeSetRollers(0.0), // turn off the rollers new HoodSetAbs(Hood.STARTING_POSITION))); } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java index 5b72bb4..183bec7 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java @@ -8,6 +8,8 @@ package org.mayheminc.robot2020.commands; import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.ShooterWheel; + import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -27,7 +29,7 @@ public ShooterCeaseFire() { // (feed roller, chimney, revolver, and shooter wheels) addCommands(new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0), - new ShooterWheelSetVBus(0.0), new Wait(0.1))); + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new Wait(0.1))); // Lower the hood now that we're done shooting addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java index 2a365da..07f6413 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java @@ -7,6 +7,9 @@ package org.mayheminc.robot2020.commands; +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -22,11 +25,11 @@ public ShooterFiringSequence(double waitDuration) { // super(new FooCommand(), new BarCommand()); super(); - // Turn off compressor while actively shooting. - addCommands(new AirCompressorPause()); - - // one last aim at the target before starting shooting. - addCommands(new ShooterAimToTarget()); + // shooting. + addCommands(new ParallelCommandGroup( // prepare for shooting, + new AirCompressorPause(), // Turn off compressor while actively shooting, + new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), // ensure intake is lowered, + new ShooterAimToTarget())); // take one last aim at the target before starting addCommands(new ParallelRaceGroup(new ShooterWheelSetToTarget(true), new TurretAimToTargetContinuously())); @@ -36,7 +39,7 @@ public ShooterFiringSequence(double waitDuration) { addCommands(new ParallelRaceGroup( // new TurretAimToTargetContinuously(), // continue aiming while shooting new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), - new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), new Wait(waitDuration))); + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.5)), new Wait(waitDuration))); // turn off the feeder, chimney, and revolver, ending after 0.1 seconds addCommands(new ShooterCeaseFire()); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java index db7c281..c9bb1d8 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -20,9 +20,10 @@ public class ShooterWheel extends SubsystemBase implements PidTunerObject { private final double SECONDS_PER_MINUTE = 60.0; private final double HUNDRED_MS_PER_SECOND = 10.0; - public static final double SHOOTER_WHEEL_INITIATION_LINE_SPEED = 3000.0; - public static final double SHOOTER_WHEEL_TRENCH_FRONT_SPEED = 3400.0; - private static final double MAX_SPEED_RPM = 3500; + public static final double IDLE_SPEED = 1500.0; + public static final double INITIATION_LINE_SPEED = 3000.0; + public static final double TRENCH_FRONT_SPEED = 3400.0; + public static final double MAX_SPEED_RPM = 3600; double m_targetSpeedRPM; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index 4d0cef4..607e07d 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -206,7 +206,13 @@ private double getHoodFromY() { * @return */ private double getWheelSpeedFromY() { - return 2618 + -1939 * m_bestY + 3583 * m_bestY * m_bestY; + double computedWheelSpeed = 2618 + -1939 * m_bestY + 3583 * m_bestY * m_bestY; + if (computedWheelSpeed < ShooterWheel.IDLE_SPEED) { + computedWheelSpeed = ShooterWheel.IDLE_SPEED; + } else if (computedWheelSpeed > ShooterWheel.MAX_SPEED_RPM) { + computedWheelSpeed = ShooterWheel.MAX_SPEED_RPM; + } + return computedWheelSpeed; } /** From 22d6fc5bb69f5c4ca089aecedcfe82b77f3f5cfa Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Fri, 28 Feb 2020 22:10:38 -0500 Subject: [PATCH 084/121] Increased speed of revolver during ShooterFiringSequence to 100% to help increase firing rate. --- .../commands/ShooterFiringSequence.java | 11 ++++++----- ...gIsOnTarget.java => TurretIsOnTarget.java} | 19 +++---------------- 2 files changed, 9 insertions(+), 21 deletions(-) rename src/main/java/org/mayheminc/robot2020/commands/{TargetingIsOnTarget.java => TurretIsOnTarget.java} (62%) diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java index 07f6413..ec961ff 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java @@ -25,21 +25,22 @@ public ShooterFiringSequence(double waitDuration) { // super(new FooCommand(), new BarCommand()); super(); - // shooting. + // Prepare for shooting. addCommands(new ParallelCommandGroup( // prepare for shooting, - new AirCompressorPause(), // Turn off compressor while actively shooting, + new AirCompressorPause(), // turn off compressor while actively shooting, new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), // ensure intake is lowered, - new ShooterAimToTarget())); // take one last aim at the target before starting + new ShooterAimToTarget())); // and aim at the target (azimuth and elevation). + // prior command established aim; turn on the shooter wheels and maintain turret addCommands(new ParallelRaceGroup(new ShooterWheelSetToTarget(true), new TurretAimToTargetContinuously())); // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the - // revolver turntable, shoot for specified duration + // revolver turntable, shoot for specified duration. // TODO: should really shoot until no balls detected any more addCommands(new ParallelRaceGroup( // new TurretAimToTargetContinuously(), // continue aiming while shooting new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), - new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.5)), new Wait(waitDuration))); + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), new Wait(waitDuration))); // turn off the feeder, chimney, and revolver, ending after 0.1 seconds addCommands(new ShooterCeaseFire()); diff --git a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TurretIsOnTarget.java similarity index 62% rename from src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java rename to src/main/java/org/mayheminc/robot2020/commands/TurretIsOnTarget.java index dfd9b68..b230af5 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TargetingIsOnTarget.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretIsOnTarget.java @@ -14,11 +14,11 @@ import edu.wpi.first.wpilibj2.command.CommandBase; -public class TargetingIsOnTarget extends CommandBase { +public class TurretIsOnTarget extends CommandBase { /** * Creates a new TargetingIsOnTarget. */ - public TargetingIsOnTarget() { + public TurretIsOnTarget() { // Use addRequirements() here to declare subsystem dependencies. } @@ -30,12 +30,6 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // double bearingToTarget = RobotContainer.targeting.getBearingToTarget(); - // double rangeToTarget = RobotContainer.targeting.getRangeToTarget(); - - // RobotContainer.shooter.setTurretPositionRel(RobotContainer.shooter.getTurretPosition() - // + bearingToTarget); - // RobotContainer.shooter.setShooterWheelSpeed(Targeting.convertRangeToWheelSpeed(rangeToTarget)); } // Called once the command ends or is interrupted. @@ -46,16 +40,9 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - // boolean bearingGood = Math.abs(RobotContainer.targeting.getBearingToTarget()) - // < 2; - // boolean wheelsGood = - // Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget()) - // - RobotContainer.shooter.getShooterWheelSpeed()) < 100; - // return bearingGood && wheelsGood; - double targetPos = RobotContainer.targeting.getDesiredAzimuth(); double turretPos = RobotContainer.turret.getPosition(); - return ( Math.abs( targetPos - turretPos) < 50); + return (Math.abs(targetPos - turretPos) < 50); } } From 7a4e3ee4daf58df639efee751956d0ba4159c2a2 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Fri, 28 Feb 2020 23:21:30 -0500 Subject: [PATCH 085/121] Updated autonomous programs to pre-start shooter only up to IDLE_SPEED (instead of INITIATION_LINE_SPEED) in order to avoid vibration during targeting. --- .../mayheminc/robot2020/autonomousroutines/StartBWShoot3.java | 2 +- .../mayheminc/robot2020/autonomousroutines/StartBWTrench.java | 2 +- .../robot2020/autonomousroutines/StartFWRendezvous.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java index 711ba47..91950ca 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java @@ -27,7 +27,7 @@ public StartBWShoot3() { // is turned towards the target addCommands(new ParallelCommandGroup( // run the following commands in parallel: new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), - new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java index 08f065c..0f05e3c 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java @@ -49,7 +49,7 @@ public StartBWTrench(double extraDistance) { // drive diagonally over towards the shooting position, while turning on shooter // wheels, raising the hood, and re-aiming the turret - addCommands(new ParallelCommandGroup(new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), + addCommands(new ParallelCommandGroup(new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)), new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160))); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java index 7e87398..53cc520 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java @@ -32,7 +32,7 @@ public StartFWRendezvous() { // is turned towards the target addCommands(new ParallelCommandGroup( // run the following commands in parallel: new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), - new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new HoodSetAbs(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); From f2e2406d1f8d18bc634e88d9a78e5ee7e7091f76 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Sat, 29 Feb 2020 22:59:09 -0500 Subject: [PATCH 086/121] Changes from last day of Granite State District event. Added semi-automatic firing sequence for "close shots," fixed intake constants to be static, tweaks to autonomous programs and other miscellaneous tweaks. --- .../mayheminc/robot2020/RobotContainer.java | 29 +++++------ .../StartBWDriveOnlyToRP.java | 5 +- .../StartBWDriveOnlyToWall.java | 5 +- .../autonomousroutines/StartBWOppTrench.java | 17 ++++--- .../autonomousroutines/StartBWShoot3.java | 5 +- .../autonomousroutines/StartBWTrench.java | 6 +-- .../StartFWDriveOnlyToRP.java | 5 +- .../StartFWDriveOnlyToWall.java | 5 +- .../autonomousroutines/StartFWRendezvous.java | 9 ++-- .../autonomousroutines/StartFWShoot3.java | 4 +- .../robot2020/commands/ShooterCeaseFire.java | 5 +- .../commands/ShooterCloseFiringSequence.java | 51 +++++++++++++++++++ .../commands/ShooterFiringSequence.java | 4 +- .../mayheminc/robot2020/subsystems/Hood.java | 1 + .../robot2020/subsystems/Intake.java | 49 +++++++++++------- .../robot2020/subsystems/ShooterWheel.java | 1 + .../robot2020/subsystems/Targeting.java | 19 ++++++- 17 files changed, 154 insertions(+), 66 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 8a93143..a26a7de 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -85,20 +85,15 @@ private void configureDefaultCommands() { // intake.setDefaultCommand(new IntakeExtenderVBus()); revolver.setDefaultCommand(new RevolverDefault()); - // TODO: Figure out if the current approach of "AirCompressorDefault()" is the - // way to go for compressor control. // KBS doesn't think the below is the right way to have the compressor be on "by - // default" because - // it would require there to always be a command running to keep the compressor - // off. However, that - // is a good way to ensure it doesn't get left off by accident. Not quite sure - // how to handle this; + // default" because it would require there to always be a command running to + // keep the compressor off. + // However, that is a good way to ensure it doesn't get left off by accident. + // Not quite sure how to handle this; // would really rather that other commands which need the compressor off (such - // as a high-power command - // which wants all the battery power available) would turn the compressor off - // when the command starts - // and off when the command ends.) Then again, maybe the "defaultCommand" is a - // good way to do this + // as a high-power command which wants all the battery power available) would + // turn the compressor off when the command starts and off when the command + // ends.) Then again, maybe the "defaultCommand" is a good way to do this // and I just don't understand the style yet. // compressor.setDefaultCommand(new AirCompressorDefault()); } @@ -206,7 +201,11 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0)); DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); - DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new ShooterCloseFiringSequence(60.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); + + // DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new + // FeederSet(1.0)); } @@ -215,9 +214,9 @@ private void configureOperatorStickButtons() { private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new RevolverSetTurntable(0.2)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(Intake.PIVOT_DOWN)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(1.0)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(Intake.PIVOT_UP)); // new ShooterSetWheel(1000)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySet(1.0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java index d6b7ef3..7d44a6c 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java @@ -8,8 +8,9 @@ package org.mayheminc.robot2020.autonomousroutines; import org.mayheminc.robot2020.commands.*; -import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + import edu.wpi.first.wpilibj2.command.*; public class StartBWDriveOnlyToRP extends SequentialCommandGroup { @@ -22,7 +23,7 @@ public StartBWDriveOnlyToRP() { addCommands(new DriveZeroGyro(180.0)); // lower the intake - addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // then, drive to the rendezvous point addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12, 180)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java index 8978dbd..196046e 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java @@ -8,8 +8,9 @@ package org.mayheminc.robot2020.autonomousroutines; import org.mayheminc.robot2020.commands.*; -import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + import edu.wpi.first.wpilibj2.command.*; public class StartBWDriveOnlyToWall extends SequentialCommandGroup { @@ -22,7 +23,7 @@ public StartBWDriveOnlyToWall() { addCommands(new DriveZeroGyro(180.0)); // lower the intake - addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // then, drive towards the wall addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 40, 180)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java index dc3ae7d..a256b99 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java @@ -7,10 +7,10 @@ package org.mayheminc.robot2020.autonomousroutines; -import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; import org.mayheminc.robot2020.subsystems.ShooterWheel; import org.mayheminc.robot2020.subsystems.Turret; @@ -24,10 +24,11 @@ public StartBWOppTrench() { addCommands(new DriveZeroGyro(180.0)); - // lower the intake and turn it on before driving forwards - addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); - addCommands(new IntakeSetRollers(-1.0)); + // lower the intake and wait for it to be on before turning on rollers or + // driving forwards + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); addCommands(new Wait(2.5)); + addCommands(new IntakeSetRollers(-1.0)); // make sure the hood is down addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); @@ -37,7 +38,7 @@ public StartBWOppTrench() { // now driven to get the balls from opposing trench addCommands(new Wait(0.8), // wait for last two balls to get into robot - new IntakeSetRollers(0), // turn off the intake + // new IntakeSetRollers(0), // slow down the intake new DriveStraightOnHeading(-0.4, DistanceUnits.INCHES, 36, 180)); // start backing up // slowly @@ -46,7 +47,7 @@ public StartBWOppTrench() { // in shooting position, prepare everything for shooting addCommands(new ParallelCommandGroup( // run the following commands in parallel: - new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION + 3000.0), new TurretSetAbs((105.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); @@ -57,13 +58,13 @@ public StartBWOppTrench() { addCommands(new Wait(0.2)); // use the "one button" firing sequence - addCommands(new ShooterFiringSequence(5.0)); + addCommands(new ShooterFiringSequence(6.0)); // turn the shooter wheel and intake off now that the shooting is all done addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED)); addCommands(new IntakeSetRollers(0.0)); // turn the wheel off now that the shooting is all done - addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); + addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java index 91950ca..184ae4c 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java @@ -7,9 +7,9 @@ package org.mayheminc.robot2020.autonomousroutines; -import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; import org.mayheminc.robot2020.subsystems.ShooterWheel; import org.mayheminc.robot2020.subsystems.Turret; @@ -26,8 +26,7 @@ public StartBWShoot3() { // first, lower the intake, start the shooter wheel, and wait until the turret // is turned towards the target addCommands(new ParallelCommandGroup( // run the following commands in parallel: - new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), - new ShooterWheelSet(ShooterWheel.IDLE_SPEED), + new IntakeSetPosition(Intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java index 0f05e3c..3fca145 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java @@ -39,13 +39,13 @@ public StartBWTrench(double extraDistance) { // now driven to the balls at far end of trench addCommands(new Wait(0.8), // wait for last two balls to get into robot - new IntakeSetRollers(0), // turn off the intake new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 12, 180)); // start backing up // slowly // after getting all three balls, go back to shooting position // first, make sure we drive straight out from under the control panel addCommands(new DriveStraightOnHeading(-0.6, DistanceUnits.INCHES, 8 + extraDistance, 180)); + addCommands(new IntakeSetRollers(0.0)); // turn off the intake in case it has been stalled for a while // drive diagonally over towards the shooting position, while turning on shooter // wheels, raising the hood, and re-aiming the turret @@ -67,7 +67,7 @@ public StartBWTrench(double extraDistance) { addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED)); addCommands(new IntakeSetRollers(0.0)); - // turn the wheel off now that the shooting is all done - addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); + // put the hood down now that the shooting is all done + addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java index 231b8b1..021b95f 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java @@ -8,8 +8,9 @@ package org.mayheminc.robot2020.autonomousroutines; import org.mayheminc.robot2020.commands.*; -import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + import edu.wpi.first.wpilibj2.command.*; public class StartFWDriveOnlyToRP extends SequentialCommandGroup { @@ -22,7 +23,7 @@ public StartFWDriveOnlyToRP() { addCommands(new DriveZeroGyro(0.0)); // lower the intake - addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // then, drive to the RP addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java index 972963a..7a4d19a 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java @@ -8,8 +8,9 @@ package org.mayheminc.robot2020.autonomousroutines; import org.mayheminc.robot2020.commands.*; -import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + import edu.wpi.first.wpilibj2.command.*; public class StartFWDriveOnlyToWall extends SequentialCommandGroup { @@ -22,7 +23,7 @@ public StartFWDriveOnlyToWall() { addCommands(new DriveZeroGyro(0.0)); // lower the intake - addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // then, drive to the RP addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 40, 0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java index 53cc520..67efa87 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java @@ -7,10 +7,10 @@ package org.mayheminc.robot2020.autonomousroutines; -import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; import org.mayheminc.robot2020.subsystems.ShooterWheel; import org.mayheminc.robot2020.subsystems.Turret; @@ -25,14 +25,13 @@ public StartFWRendezvous() { // start out facing in the normal direction addCommands(new DriveZeroGyro(0.0)); - addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // shoot the 3 balls we started with // first, lower the intake, start the shooter wheel, and wait until the turret // is turned towards the target addCommands(new ParallelCommandGroup( // run the following commands in parallel: - new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), - new ShooterWheelSet(ShooterWheel.IDLE_SPEED), + new IntakeSetPosition(Intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new HoodSetAbs(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); @@ -43,7 +42,7 @@ public StartFWRendezvous() { // now that we are clear of other robots, lower the intake while backing up // further - addCommands(new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // raise the hood a little to shoot from this increased distance addCommands(new HoodSetAbsWhileHeld((Hood.INITIATION_LINE_POSITION + Hood.TRENCH_MID_POSITION) / 2.0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java index d5d16a9..1d5d0d4 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java @@ -7,9 +7,9 @@ package org.mayheminc.robot2020.autonomousroutines; -import org.mayheminc.robot2020.RobotContainer; import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; import org.mayheminc.robot2020.subsystems.ShooterWheel; import org.mayheminc.robot2020.subsystems.Turret; @@ -26,7 +26,7 @@ public StartFWShoot3() { // first, lower the intake, start the shooter wheel, and wait until the turret // is turned towards the target addCommands(new ParallelCommandGroup( // run the following commands in parallel: - new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), + new IntakeSetPosition(Intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java index 183bec7..9b03987 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java @@ -8,6 +8,7 @@ package org.mayheminc.robot2020.commands; import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; import org.mayheminc.robot2020.subsystems.ShooterWheel; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; @@ -31,7 +32,9 @@ public ShooterCeaseFire() { addCommands(new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0), new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new Wait(0.1))); - // Lower the hood now that we're done shooting + // Lower the hood and intake now that we're done shooting addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // ensure intake is lowered,); + addCommands(new IntakeSetRollers(0.0)); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java new file mode 100644 index 0000000..62d619c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; +import org.mayheminc.robot2020.subsystems.ShooterWheel; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShooterCloseFiringSequence extends SequentialCommandGroup { + /** + * Creates a new ShooterReadyAimFire. + */ + public ShooterCloseFiringSequence(double waitDuration) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + // Prepare for shooting. + addCommands(new IntakeSetPosition(Intake.PIVOT_SHOOTING)); // move intake to "shooting position" + addCommands(new ParallelCommandGroup( // prepare for shooting, + new AirCompressorPause() // turn off compressor while actively shooting, + // , new ShooterAimToTarget() // and aim at the target (azimuth and elevation). + )); + + // no aiming when up close; just turn on the shooter wheels and raise the hood + addCommands(new ShooterWheelSet(ShooterWheel.CLOSE_SHOOTING_SPEED), // shooter wheel manual speed + new HoodSetAbs(Hood.CLOSE_SHOOTING_POSITION)); + + // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the + // revolver turntable, shoot for specified duration. + // TODO: should really shoot until no balls detected any more + addCommands(new ParallelRaceGroup( // + new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), new Wait(waitDuration))); + + // turn off the feeder, chimney, and revolver, ending after 0.1 seconds + addCommands(new ShooterCeaseFire()); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java index ec961ff..7143104 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java @@ -7,7 +7,7 @@ package org.mayheminc.robot2020.commands; -import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.subsystems.Intake; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; @@ -26,9 +26,9 @@ public ShooterFiringSequence(double waitDuration) { super(); // Prepare for shooting. + addCommands(new IntakeSetPosition(Intake.PIVOT_SHOOTING)); // move intake to "shooting position" addCommands(new ParallelCommandGroup( // prepare for shooting, new AirCompressorPause(), // turn off compressor while actively shooting, - new IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN), // ensure intake is lowered, new ShooterAimToTarget())); // and aim at the target (azimuth and elevation). // prior command established aim; turn on the shooter wheels and maintain turret diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java index 5f3f2ce..04bd78c 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java @@ -20,6 +20,7 @@ public class Hood extends SubsystemBase implements PidTunerObject { public final static double STARTING_POSITION = 0; public final static double TARGET_ZONE_POSITION = 5000; + public final static double CLOSE_SHOOTING_POSITION = 30000; public final static double INITIATION_LINE_POSITION = 65000; public final static double TRENCH_MID_POSITION = 80000; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 6a64789..c80eb5f 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -24,11 +24,12 @@ public class Intake extends SubsystemBase implements PidTunerObject { private final int PIVOT_CLOSE_ENOUGH = 50; - private final VictorSPX rollerTalon = new VictorSPX(Constants.Talon.INTAKE_ROLLERS); + private final VictorSPX rollersTalon = new VictorSPX(Constants.Talon.INTAKE_ROLLERS); private final MayhemTalonSRX pivotTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_PIVOT); - private final int PIVOT_ZERO_POSITION = 900; - public final double PIVOT_UP = 900.0; - public final double PIVOT_DOWN = 0.0; + private static final int PIVOT_ZERO_POSITION = 950; + public static final double PIVOT_UP = PIVOT_ZERO_POSITION; + public static final double PIVOT_SHOOTING = 100.0; + public static final double PIVOT_DOWN = 0.0; private static final double HORIZONTAL_HOLD_OUTPUT = 0.00; private static final double MAX_PID_MOVEMENT_TIME_SEC = 10.0; @@ -37,21 +38,22 @@ enum PivotMode { MANUAL_MODE, PID_MODE, }; - PivotMode m_mode = PivotMode.MANUAL_MODE; - boolean m_isMoving; - double m_targetPosition; - double m_feedForward; - Timer m_pidTimer = new Timer(); + private PivotMode m_mode = PivotMode.MANUAL_MODE; + private boolean m_isMoving; + private double m_desiredRollersPower = 0.0; + private double m_targetPosition; + private double m_feedForward; + private Timer m_pidTimer = new Timer(); /** * Creates a new Intake. */ public Intake() { - rollerTalon.setNeutralMode(NeutralMode.Coast); - rollerTalon.configNominalOutputForward(+0.0f); - rollerTalon.configNominalOutputReverse(0.0); - rollerTalon.configPeakOutputForward(+12.0); - rollerTalon.configPeakOutputReverse(-12.0); + rollersTalon.setNeutralMode(NeutralMode.Coast); + rollersTalon.configNominalOutputForward(+0.0f); + rollersTalon.configNominalOutputReverse(0.0); + rollersTalon.configPeakOutputForward(+12.0); + rollersTalon.configPeakOutputReverse(-12.0); configPivotMotor(pivotTalon); } @@ -99,7 +101,8 @@ public void zero() { * @param power */ public void setRollers(double power) { - rollerTalon.set(ControlMode.PercentOutput, power); + m_desiredRollersPower = power; + // rollersTalon.set(ControlMode.PercentOutput, power); } public void setPivot(Double b) { @@ -117,10 +120,10 @@ public boolean isPivotAtPosition() { @Override public void periodic() { // This method will be called once per scheduler run - updateSmartDashBoard(); updateSensorPosition(); updatePivotPower(); + updateRollersPower(); } private void updatePivotPower() { @@ -134,7 +137,7 @@ private void updatePivotPower() { // if the current position is closer to PIVOT UP than PIVOT DOWN, apply a little // positive power. - if (pivotTalon.getPosition() > PIVOT_UP / 2) { + if (pivotTalon.getPosition() > (PIVOT_UP / 2.0)) { setPivotVBus(+0.05); } else { // we are close to the PIVOT DOWN, so apply a little negative power. setPivotVBus(-0.05); @@ -145,6 +148,16 @@ private void updatePivotPower() { } } + void updateRollersPower() { + if (pivotTalon.getPosition() > (PIVOT_UP / 2.0)) { + // turn off the rollers automatically if the pivot is too high + rollersTalon.set(ControlMode.PercentOutput, 0.0); + } else { + // set the rollers as requested if the pivot is low enough + rollersTalon.set(ControlMode.PercentOutput, m_desiredRollersPower); + } + } + void updateSensorPosition() { int m_currentPosition = pivotTalon.getPosition(); double m_angleInDegrees = positionToDegrees(m_currentPosition); @@ -173,7 +186,7 @@ public void updateSmartDashBoard() { SmartDashboard.putBoolean("Intake Is Moving", m_isMoving); SmartDashboard.putBoolean("Intake PID Mode", (m_mode == PivotMode.PID_MODE)); - SmartDashboard.putNumber("Intake Rollers", rollerTalon.getMotorOutputPercent()); + SmartDashboard.putNumber("Intake Rollers", rollersTalon.getMotorOutputPercent()); } public void setPivotVBus(double VBus) { diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java index c9bb1d8..e95ae74 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -21,6 +21,7 @@ public class ShooterWheel extends SubsystemBase implements PidTunerObject { private final double HUNDRED_MS_PER_SECOND = 10.0; public static final double IDLE_SPEED = 1500.0; + public static final double CLOSE_SHOOTING_SPEED = 3000.0; public static final double INITIATION_LINE_SPEED = 3000.0; public static final double TRENCH_FRONT_SPEED = 3400.0; public static final double MAX_SPEED_RPM = 3600; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index 607e07d..20ec9f3 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -33,6 +33,10 @@ public class Targeting extends SubsystemBase { // Define the "target location" to be halfway from left to right private final double CENTER_OF_TARGET_X = 0.475; + private final double BEST_Y_CLOSE_THRESHOLD = 0.1; + private final double CLOSE_WHEEL_SPEED = 3000.0; + private final double CLOSE_HOOD_ANGLE = 30000.0; + // Calculate ticks per degree. // encoder ticks * turret pulley teeth / drive pulley teeth / 360 degrees private final double TICKS_PER_DEGREE = (4096.0 * 225.0 / 18.0 / 360.0); // was 6300 / 45 @@ -197,7 +201,14 @@ public double findDesiredAzimuth(double X, double Y, double tilt, double area) { * @return */ private double getHoodFromY() { - return 105874 + -407324 * m_bestY + 881911 * m_bestY * m_bestY + -506286 * m_bestY * m_bestY * m_bestY; + // below is the "curve fit" for the "long shot" + + if (m_bestY < BEST_Y_CLOSE_THRESHOLD) { + // too close for the lob shot, switch to the bullet shot + return CLOSE_HOOD_ANGLE; + } else { + return 105874 + -407324 * m_bestY + 881911 * m_bestY * m_bestY + -506286 * m_bestY * m_bestY * m_bestY; + } } /** @@ -207,6 +218,12 @@ private double getHoodFromY() { */ private double getWheelSpeedFromY() { double computedWheelSpeed = 2618 + -1939 * m_bestY + 3583 * m_bestY * m_bestY; + + if (m_bestY < BEST_Y_CLOSE_THRESHOLD) { + // too close for the lob shot, switch to the bullet shot + computedWheelSpeed = CLOSE_WHEEL_SPEED; + } + if (computedWheelSpeed < ShooterWheel.IDLE_SPEED) { computedWheelSpeed = ShooterWheel.IDLE_SPEED; } else if (computedWheelSpeed > ShooterWheel.MAX_SPEED_RPM) { From d01872ae1dbf12db66f764c86668f6bd831e399c Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Tue, 3 Mar 2020 23:46:16 -0500 Subject: [PATCH 087/121] Changes from first night after GSD event: (1) added current limits to drive motor code, (2) tuned heading correction code in drive to drive straight without wandering, and (3) added some "DriveStraightOnHeading()" commands to the driver pad "start" and "back" buttons for testing. --- .../mayheminc/robot2020/RobotContainer.java | 6 ++- .../autonomousroutines/StartBWTrench.java | 6 +-- .../mayheminc/robot2020/subsystems/Drive.java | 45 ++++++++++++++----- 3 files changed, 40 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index a26a7de..18e3c0b 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -19,6 +19,7 @@ import org.mayheminc.robot2020.autonomousroutines.*; import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; import org.mayheminc.robot2020.subsystems.*; /** @@ -196,14 +197,15 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0)); DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED)); - DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whileHeld(new DriveStraight(0.1)); - DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0)); DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new ShooterCloseFiringSequence(60.0)); DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); + DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whenPressed(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 240, 0)); + DRIVER_PAD.DRIVER_PAD_START_BUTTON.whenPressed(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 240, 0)); + // DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new // FeederSet(1.0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java index 3fca145..301527b 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java @@ -26,7 +26,7 @@ public StartBWTrench(double extraDistance) { addCommands(new StartBWShoot3()); // then, drive to the trench entrance (jog left a little to get there) - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 46, 140)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 30, 140)); // pick up balls while heading down the trench. addCommands(new ParallelCommandGroup( @@ -35,7 +35,7 @@ public StartBWTrench(double extraDistance) { // ensure the hood is down new HoodSetAbsWhileHeld(Hood.STARTING_POSITION), // drive the path under the control panel to the end - new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 140 + extraDistance, 180))); + new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 148 + extraDistance, 180))); // now driven to the balls at far end of trench addCommands(new Wait(0.8), // wait for last two balls to get into robot @@ -44,7 +44,7 @@ public StartBWTrench(double extraDistance) { // after getting all three balls, go back to shooting position // first, make sure we drive straight out from under the control panel - addCommands(new DriveStraightOnHeading(-0.6, DistanceUnits.INCHES, 8 + extraDistance, 180)); + addCommands(new DriveStraightOnHeading(-0.6, DistanceUnits.INCHES, 16 + extraDistance, 180)); addCommands(new IntakeSetRollers(0.0)); // turn off the intake in case it has been stalled for a while // drive diagonally over towards the shooting position, while turning on shooter diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index a5392f0..d9560bc 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -51,8 +51,9 @@ public class Drive extends SubsystemBase implements PidTunerObject { // NavX parameters private double m_desiredHeading = 0.0; private boolean m_useHeadingCorrection = true; - private static final double HEADING_PID_P = 0.007; // was 0.030 in 2019 for HIGH_GEAR; was 0.007 in early 2020 - private static final double HEADING_PID_D = 0.080; // was 0.04 in 2019 + private static final double HEADING_PID_P = 0.010; // was 0.007 at GSD; was 0.030 in 2019 for HIGH_GEAR + private static final double HEADING_PID_I = 0.000; // was 0.000 at GSD; was 0.000 in 2019 + private static final double HEADING_PID_D = 0.080; // was 0.080 at GSD; was 0.04 in 2019 private static final double kToleranceDegreesPIDControl = 0.2; // Drive parameters @@ -97,8 +98,8 @@ public Drive() { m_HeadingError = new PIDHeadingError(); m_HeadingError.m_Error = 0.0; m_HeadingCorrection = new PIDHeadingCorrection(); - m_HeadingPid = new PIDController(HEADING_PID_P, 0.000, HEADING_PID_D, m_HeadingError, m_HeadingCorrection, - 0.020 /* period in seconds */); + m_HeadingPid = new PIDController(HEADING_PID_P, HEADING_PID_I, HEADING_PID_D, m_HeadingError, + m_HeadingCorrection, 0.020 /* period in seconds */); m_HeadingPid.setInputRange(-180.0f, 180.0f); m_HeadingPid.setContinuous(true); // treats the input range as "continous" with wrap-around m_HeadingPid.setOutputRange(-.50, .50); // set the maximum power to correct twist @@ -110,6 +111,26 @@ public Drive() { rightFrontTalon.setNeutralMode(NeutralMode.Coast); rightRearTalon.setNeutralMode(NeutralMode.Coast); + // configure output voltages + leftFrontTalon.configNominalOutputVoltage(+0.0f, -0.0f); + leftRearTalon.configNominalOutputVoltage(+0.0f, -0.0f); + rightFrontTalon.configNominalOutputVoltage(+0.0f, -0.0f); + rightRearTalon.configNominalOutputVoltage(+0.0f, -0.0f); + leftFrontTalon.configPeakOutputVoltage(+12.0, -12.0); + leftRearTalon.configPeakOutputVoltage(+12.0, -12.0); + rightFrontTalon.configPeakOutputVoltage(+12.0, -12.0); + rightRearTalon.configPeakOutputVoltage(+12.0, -12.0); + + // configure current limits + // enabled = true + // 40 = limit (amps) + // 60 = trigger_threshold (amps) + // 0.5 = threshold time(s) + leftFrontTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5)); + leftRearTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5)); + rightFrontTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5)); + rightRearTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5)); + // set rear talons to follow their respective front talons leftRearTalon.follow(leftFrontTalon); rightRearTalon.follow(rightFrontTalon); @@ -247,13 +268,6 @@ public void setHeadingCorrectionMode(final boolean useHeadingCorrection) { } private void resetAndEnableHeadingPID() { - // if (Robot.shifter.getGear() == Shifter.HIGH_GEAR) { - m_HeadingPid.setP(HEADING_PID_P); - // } else - // { - // low gear - // m_HeadingPid.setP(HEADING_PID_P_FOR_LOW_GEAR); - // } m_HeadingPid.reset(); m_HeadingPid.enable(); } @@ -606,6 +620,8 @@ private double maintainHeading() { // check for major heading changes and take action to prevent // integral windup if there is a major heading error + // TODO: In 2020, this code was causing "wandering" with non-zero HEADING_PID_I. + // Worked around issue by setting HEADING_PID_I = 0 if (Math.abs(m_HeadingError.m_Error) > 10.0) { if (!m_HeadingPidPreventWindup) { m_HeadingPid.setI(0.0); @@ -614,7 +630,7 @@ private double maintainHeading() { } } else { m_HeadingPidPreventWindup = false; - m_HeadingPid.setI(0.001); + m_HeadingPid.setI(HEADING_PID_I); } return headingCorrection; @@ -676,6 +692,11 @@ public void updateSmartDashboard() { SmartDashboard.putNumber("Left Talon Output Voltage", leftFrontTalon.getMotorOutputVoltage()); SmartDashboard.putNumber("Right Talon Output Voltage", rightFrontTalon.getMotorOutputVoltage()); + SmartDashboard.putNumber("LF Falcon Supply Current", leftFrontTalon.getSupplyCurrent()); + SmartDashboard.putNumber("LR Falcon Supply Current", leftRearTalon.getSupplyCurrent()); + SmartDashboard.putNumber("RF Falcon Supply Current", rightFrontTalon.getSupplyCurrent()); + SmartDashboard.putNumber("RR Falcon Supply Current", rightRearTalon.getSupplyCurrent()); + SmartDashboard.putBoolean("Closed Loop Mode", m_closedLoopMode); SmartDashboard.putBoolean("Speed Racer Drive Mode", m_speedRacerDriveMode); From 2ef50e528876d4bc02fc2d2e21b4bcb6b07ab7f6 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Wed, 4 Mar 2020 19:47:34 -0500 Subject: [PATCH 088/121] Update TrenchAuto.java --- .../org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java index 4ba9b58..bd43c65 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java @@ -41,6 +41,8 @@ public TrenchAuto() { addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0)); addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 6, 0)); + addCommands(new ShooterReadyAimFire(6.0)); + // turn the wheel off now that the shooting is all done addCommands(new ShooterSetWheel(0.0)); From c20fa9a4e1f2167cda10dd18f53caa9ba47fd1f5 Mon Sep 17 00:00:00 2001 From: Ihedgehogs <58241515+Ihedgehogs@users.noreply.github.com> Date: Wed, 4 Mar 2020 19:47:43 -0500 Subject: [PATCH 089/121] Revert "Update TrenchAuto.java" This reverts commit 2ef50e528876d4bc02fc2d2e21b4bcb6b07ab7f6. --- .../org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java index bd43c65..4ba9b58 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TrenchAuto.java @@ -41,8 +41,6 @@ public TrenchAuto() { addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0)); addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 6, 0)); - addCommands(new ShooterReadyAimFire(6.0)); - // turn the wheel off now that the shooting is all done addCommands(new ShooterSetWheel(0.0)); From f0943672ed91c25462d462e5040240110e795213 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Tue, 10 Mar 2020 22:10:23 -0400 Subject: [PATCH 090/121] Vendor Library Updates: WPILib to 2020.3.2, CTRE Phoenix to 5.18.2, and navX to 3.1.413 version. No other code changes. --- build.gradle | 2 +- vendordeps/Phoenix.json | 30 +++++++++++++++--------------- vendordeps/navx_frc.json | 8 ++++---- 3 files changed, 20 insertions(+), 20 deletions(-) diff --git a/build.gradle b/build.gradle index becc3d0..9eb9d8f 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2020.2.2" + id "edu.wpi.first.GradleRIO" version "2020.3.2" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 8b25850..aa5554e 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.17.6", + "version": "5.18.2", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.17.6" + "version": "5.18.2" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.17.6" + "version": "5.18.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.6", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -35,7 +35,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.6", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -47,7 +47,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.6", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.6", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -69,7 +69,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.6", + "version": "5.18.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -83,7 +83,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.17.6", + "version": "5.18.2", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -97,7 +97,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.17.6", + "version": "5.18.2", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -111,7 +111,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.17.6", + "version": "5.18.2", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -125,7 +125,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "diagnostics", - "version": "5.17.6", + "version": "5.18.2", "libName": "CTRE_PhoenixDiagnostics", "headerClassifier": "headers", "sharedLibrary": false, @@ -139,7 +139,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "canutils", - "version": "5.17.6", + "version": "5.18.2", "libName": "CTRE_PhoenixCanutils", "headerClassifier": "headers", "sharedLibrary": false, @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", - "version": "5.17.6", + "version": "5.18.2", "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -165,7 +165,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.17.6", + "version": "5.18.2", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json index f7c0fa4..dca1d82 100644 --- a/vendordeps/navx_frc.json +++ b/vendordeps/navx_frc.json @@ -1,7 +1,7 @@ { "fileName": "navx_frc.json", "name": "KauaiLabs_navX_FRC", - "version": "3.1.405", + "version": "3.1.413", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "mavenUrls": [ "https://repo1.maven.org/maven2/" @@ -11,7 +11,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-java", - "version": "3.1.405" + "version": "3.1.413" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-cpp", - "version": "3.1.405", + "version": "3.1.413", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, @@ -28,7 +28,7 @@ "binaryPlatforms": [ "linuxathena", "linuxraspbian", - "windowsx86-x64" + "windowsx86-64" ] } ] From 3cdef584ac6a786a85bda790cfc02ab99ea7d578 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 3 Dec 2020 19:38:44 -0500 Subject: [PATCH 091/121] Changes from practice field during fall 2020 to change field of view of 1CGN camera, to add a frame count to the data stream between coprocessor and robot, and to "perturb" the vision desired azimuth on SmartDashboard display to force updates. --- .../robot2020/subsystems/Targeting.java | 35 +++++++++++-------- 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index 20ec9f3..36d6c62 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -28,7 +28,13 @@ public class Targeting extends SubsystemBase { // Horizontal FOV = 70.42 // Vertical FOV = 43.3 // NOTE: 76.5 horizontal FOV determined empirically by Ken on 2/22/2020 - private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 76.5; + // private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 76.5; + + // oCam 1CGN Field of View Information: + // FOV = 65.0 + // NOTE: 86.5 horizontal FOV determined empirically by Ken, Amy, and Caleb on + // 9/24/2020 + private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 63.0; // was 86.5 on 4 Nov 2020 // Define the "target location" to be halfway from left to right private final double CENTER_OF_TARGET_X = 0.475; @@ -105,20 +111,20 @@ public void update() { // get the latest output from the targeting camera m_target_array = SmartDashboard.getNumberArray("target", ARRAY_OF_NEG_ONE); - if (m_target_array == null || m_target_array.length == 0) { + if (m_target_array == null || m_target_array.length == 0 || m_target_array.length == 1) { // this means the key is found, but is empty m_bestX = 0.0; m_bestY = 0.0; m_tilt = 0.0; m_area = 0.0; // m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); - } else if (m_target_array[0] < 0.0) { - // this means the array has no valid data. Set m_xError = 0.0 - m_bestX = 0.0; - m_bestY = 0.0; - m_tilt = 0.0; - m_area = 0.0; - // m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); + // } else if (m_target_array[0] < 0.0) { + // // this means the array has no valid data. Set m_xError = 0.0 + // m_bestX = 0.0; + // m_bestY = 0.0; + // m_tilt = 0.0; + // m_area = 0.0; + // // m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); } else { // We have a valid data array. // There are three different situations: @@ -128,10 +134,11 @@ public void update() { // Handle each of them separately; // we need the results in "bestXError" and "bestY" - m_bestX = m_target_array[0]; // get the x-value - m_bestY = m_target_array[1]; // get the y-value - m_tilt = m_target_array[2]; - m_area = m_target_array[3]; + // NOTEL m_target_array[0] is now the frame count, but we're not using it yet + m_bestX = m_target_array[1]; // get the x-value + m_bestY = m_target_array[2]; // get the y-value + m_tilt = m_target_array[3]; + m_area = m_target_array[4]; m_desiredAzimuth = findDesiredAzimuth(m_bestX, m_bestY, m_tilt, m_area); m_desiredHood = getHoodFromY(); @@ -191,7 +198,7 @@ public double findDesiredAzimuth(double X, double Y, double tilt, double area) { desiredAzimuth = ticksError + RobotContainer.turret.getAzimuthForCapturedImage(); // Update SmartDashboard SmartDashboard.putNumber("Vision Angle Error", angleError); - SmartDashboard.putNumber("Vision Desired Azimuth", desiredAzimuth); + SmartDashboard.putNumber("Vision Desired Azimuth", desiredAzimuth + Math.random()); return desiredAzimuth; } From 1fe1e7290567eb206fdb601d2ba7358353d89c4a Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 3 Dec 2020 19:40:10 -0500 Subject: [PATCH 092/121] Change camera lag --- .../java/org/mayheminc/robot2020/subsystems/Turret.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java index 42c990d..d77e71d 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -71,8 +71,12 @@ public void periodic() { updateHistory(); } - // KBS: tuned below at practice field on 21 Feb 2020 via successive refinement. - private static final double CAMERA_LAG = 0.08; // .05 was best so far in 2020; used .150 in 2019 + // KBS: tuned below at practice field on 21 Feb 2020 via successive refinement + // to get 0.08 w/Logitech C920 camera. + + // KBS: tuned below at practice field on 24 Sept 2020 w/Caleb and Amy to be 0.17 + // w/1CGN camera + private static final double CAMERA_LAG = 0.17; // .05 was best so far in 2020; used .150 in 2019 private void updateHistory() { double now = Timer.getFPGATimestamp(); From 7d2c8314128afceca6d11d5b0d4d449af2a31616 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 10 Dec 2020 17:59:25 -0500 Subject: [PATCH 093/121] Changed PIDF parameters for turret to now use P=0.7 and D=7.0 (from prior P=1.0, D=I=F=0.0) to give better movement. Would still benefit from a slight non-zero I, but need to figure out how to avoid "integral windup problems" with I being non-zero. --- .../mayheminc/robot2020/subsystems/Turret.java | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java index d77e71d..e1b6192 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -42,9 +42,13 @@ public void init() { } void configureTurretTalon() { - turretTalon.config_kP(0, 1.0, 0); + // PID tuning notes: + // during competition season, used P=1.0; everything else zero + // on 3 December tuned with Caleb, Amy, Coach Streeter with P=0.7, D=7.0 + // Note: had "overshoot" issues when using I. (Tried 0.001 to 0.01) + turretTalon.config_kP(0, 0.7, 0); turretTalon.config_kI(0, 0.0, 0); - turretTalon.config_kD(0, 0.0, 0); + turretTalon.config_kD(0, 7.0, 0); turretTalon.config_kF(0, 0.0, 0); turretTalon.changeControlMode(ControlMode.Position); turretTalon.setNeutralMode(NeutralMode.Coast); @@ -91,6 +95,7 @@ public double getAzimuthForCapturedImage() { private void UpdateDashboard() { SmartDashboard.putNumber("Shooter turret pos", turretTalon.getPosition()); + SmartDashboard.putNumber("Shooter turret pos desired", m_desiredPosition); SmartDashboard.putNumber("Shooter turret vbus", turretTalon.getMotorOutputVoltage()); SmartDashboard.putNumber("Shooter turret velocity", turretTalon.getSelectedSensorVelocity(0)); } @@ -120,6 +125,13 @@ public void setPositionAbs(double pos) { * @param pos number of encoder ticks to adjust. */ public void setPositionRel(double pos) { + // Below line sets position relative to most recent "desiredPosition" but may + // have BAD side-effect if turret had just been in VBus mode (i.e. may not have + // been near prior desiredPosition) Would need to decide a safer way to know + // whether to have new relative position be relative to the current + // desiredPosition or the current actual position. + + // m_desiredPosition = m_desiredPosition + pos; m_desiredPosition = getPosition() + pos; setPositionAbs(m_desiredPosition); } From 79124e4de53577409a8884c312c147a62e454a00 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Thu, 10 Dec 2020 18:28:17 -0500 Subject: [PATCH 094/121] Make gradlew executable for Unix systems --- gradlew | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 From b212f888407debdfc0c5d0f59bea17e3c9e46960 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 10 Dec 2020 20:13:48 -0500 Subject: [PATCH 095/121] Add turret integral zone --- .../org/mayheminc/robot2020/subsystems/Turret.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java index e1b6192..f495bb5 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -19,6 +19,10 @@ public class Turret extends SubsystemBase implements PidTunerObject { private final int MIN_POSITION = -12500; // approx 90 degrees private final int MAX_POSITION = +26000; // approx 180 degrees private final int DESTINATION_TOLERANCE = 200; + + // if "at destination" want the "I" to get us as close as possible + private final int INTEGRAL_ZONE = 100; + // TICKS_PER_DEGREE computed by 4096 ticks per rotation of shaft / 1 rotation of // shaft per 18t * 225t / 1 rotation of turret @@ -47,9 +51,13 @@ void configureTurretTalon() { // on 3 December tuned with Caleb, Amy, Coach Streeter with P=0.7, D=7.0 // Note: had "overshoot" issues when using I. (Tried 0.001 to 0.01) turretTalon.config_kP(0, 0.7, 0); - turretTalon.config_kI(0, 0.0, 0); + turretTalon.config_kI(0, 0.01, 0); turretTalon.config_kD(0, 7.0, 0); turretTalon.config_kF(0, 0.0, 0); + + // experimented by adding "integral zone" for when turret is close to intended + // target + turretTalon.config_IntegralZone(0, INTEGRAL_ZONE); turretTalon.changeControlMode(ControlMode.Position); turretTalon.setNeutralMode(NeutralMode.Coast); turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); From b03d4b65ceefa515ad3981ad2507930aa77510e3 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 10 Dec 2020 21:36:32 -0500 Subject: [PATCH 096/121] Begin adding inner port targeting --- .../robot2020/subsystems/Targeting.java | 38 +++++++++++++++---- .../robot2020/subsystems/Turret.java | 13 ++++++- 2 files changed, 43 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index 36d6c62..4bb7053 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -43,14 +43,13 @@ public class Targeting extends SubsystemBase { private final double CLOSE_WHEEL_SPEED = 3000.0; private final double CLOSE_HOOD_ANGLE = 30000.0; - // Calculate ticks per degree. - // encoder ticks * turret pulley teeth / drive pulley teeth / 360 degrees - private final double TICKS_PER_DEGREE = (4096.0 * 225.0 / 18.0 / 360.0); // was 6300 / 45 - // After computing a desired azimuth, add a "fudge" offset to correct // empirically measured error. Offset should be in azimuth "ticks." private static final double AZIMUTH_CORRECTION_OFFSET = 0.0; // was -2.0 at CMP + // TODO, inner port depth in feet + private static final double INNER_PORT_DEPTH = 29.5 / 12.0; + private double m_desiredAzimuth; private double m_desiredHood; private double m_desiredWheelSpeed; @@ -140,7 +139,17 @@ public void update() { m_tilt = m_target_array[3]; m_area = m_target_array[4]; - m_desiredAzimuth = findDesiredAzimuth(m_bestX, m_bestY, m_tilt, m_area); + m_desiredAzimuth = findDesiredAzimuthOuterPort(m_bestX, m_bestY, m_tilt, m_area); + + // TODO: The following code calculates the inner port angle! + // + // double outerPortTicks = findDesiredAzimuthOuterPort(m_bestX, m_bestY, m_tilt, + // m_area); + // double outerPortDegrees = outerPortTicks / Turret.TICKS_PER_DEGREE; + + // double innerPortDegrees = getAngleToInnerPort(outerPortDegrees); + // m_desiredAzimuth = innerPortDegrees * Turret.TICKS_PER_DEGREE; + m_desiredHood = getHoodFromY(); m_desiredWheelSpeed = getWheelSpeedFromY(); } @@ -180,7 +189,7 @@ public double getDesiredWheelSpeed() { * @param area * @return */ - public double findDesiredAzimuth(double X, double Y, double tilt, double area) { + public double findDesiredAzimuthOuterPort(double X, double Y, double tilt, double area) { // Calulate angle error based on an X,Y double angleError; double ticksError; @@ -192,7 +201,7 @@ public double findDesiredAzimuth(double X, double Y, double tilt, double area) { // Find the angle error angleError = FOV_HORIZ_CAMERA_IN_DEGREES * xError; // convert the angle error into ticks - ticksError = angleError * TICKS_PER_DEGREE; + ticksError = angleError * Turret.TICKS_PER_DEGREE; // Convert angleError into a desired azimuth, using the azimuth history desiredAzimuth = ticksError + RobotContainer.turret.getAzimuthForCapturedImage(); @@ -202,6 +211,21 @@ public double findDesiredAzimuth(double X, double Y, double tilt, double area) { return desiredAzimuth; } + /** + * \(owo)/ <- hi! Computes the angle to the inner port! + * + * @return + */ + public double getAngleToInnerPort(double angleToOuterPort) { + double range = getRangeFromArea(); + double degreesToRadians = 2 * Math.PI / 360; + + double x = range * Math.sin(angleToOuterPort * degreesToRadians); + double y = range * Math.cos(angleToOuterPort * degreesToRadians); + + return Math.atan(x / (y + INNER_PORT_DEPTH)) / degreesToRadians; + } + /** * use m_bestY to get the desired hood setting for the target * diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java index f495bb5..5a36fda 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -1,6 +1,7 @@ package org.mayheminc.robot2020.subsystems; import org.mayheminc.robot2020.Constants; +import org.mayheminc.robot2020.RobotContainer; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -149,7 +150,7 @@ public void setVBus(double power) { } /** - * Get the current position of the turret. + * Get the current position of the turret (in encoder counts). * * @return */ @@ -166,6 +167,16 @@ public double getDesiredPosition() { return m_desiredPosition; } + /** + * Gets the turret's absolute position, in degrees, relative to the robot's + * heading. + * + * @return + */ + public double getGlobalTurretPosition() { + return RobotContainer.drive.getHeading() + (this.getPosition() / TICKS_PER_DEGREE); + } + /** * Return true if close enough to desired position * From 858f1505d03ceaae74c87be09756c904baa9ec60 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Thu, 10 Dec 2020 22:12:37 -0500 Subject: [PATCH 097/121] Add build workflow --- .github/workflows/build.yml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 .github/workflows/build.yml diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..c009cae --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,14 @@ +name: Build +on: + - push + - pull_request +jobs: + build: + name: Build + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: actions/setup-java@v1 + with: + java-version: '11.0.2' + - run: ./gradlew build From d440c8cc9165b52bbdffadcfc1839d4add9106e9 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Fri, 15 Jan 2021 21:46:29 -0500 Subject: [PATCH 098/121] Add DriveSlalom path --- PathWeaver/Paths/Unnamed.path | 3 ++ PathWeaver/Paths/Unnamed_0.path | 3 ++ PathWeaver/pathweaver.json | 9 ++++ .../mayheminc/robot2020/RobotContainer.java | 26 ++++++----- .../autonomousroutines/DriveSlalom.java | 45 +++++++++++++++++++ 5 files changed, 74 insertions(+), 12 deletions(-) create mode 100644 PathWeaver/Paths/Unnamed.path create mode 100644 PathWeaver/Paths/Unnamed_0.path create mode 100644 PathWeaver/pathweaver.json create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java diff --git a/PathWeaver/Paths/Unnamed.path b/PathWeaver/Paths/Unnamed.path new file mode 100644 index 0000000..93d59e1 --- /dev/null +++ b/PathWeaver/Paths/Unnamed.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +0.0,0.0,120.0,0.0,true, +120.0,-120.0,0.0,-120.0,true, diff --git a/PathWeaver/Paths/Unnamed_0.path b/PathWeaver/Paths/Unnamed_0.path new file mode 100644 index 0000000..93d59e1 --- /dev/null +++ b/PathWeaver/Paths/Unnamed_0.path @@ -0,0 +1,3 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Name +0.0,0.0,120.0,0.0,true, +120.0,-120.0,0.0,-120.0,true, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json new file mode 100644 index 0000000..5fe7ade --- /dev/null +++ b/PathWeaver/pathweaver.json @@ -0,0 +1,9 @@ +{ + "lengthUnit": "Inch", + "exportUnit": "Always Meters", + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "wheelBase": 5.0, + "gameName": "Infinite Recharge", + "outputDir": "" +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 18e3c0b..074f584 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -66,7 +66,7 @@ public RobotContainer() { pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN, - RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, drive); + RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, turret); cameraLights.set(true); } @@ -102,6 +102,7 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); + autonomousPrograms.push(/* 13 */ new DriveSlalom()); autonomousPrograms.push(/* 12 */ new StayStill()); autonomousPrograms.push(/* 11 */ new StartBWDriveOnlyToRP()); autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToWall()); @@ -174,14 +175,14 @@ private void configureDriverPadButtons() { // about -30 degrees // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new // ShooterSetTurretVBus(+0.2));// about +30 degrees - DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION)); - DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new + // HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new + // HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); - // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new - // ShooterSetTurretRel(-200.0)); - // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new - // ShooterSetTurretRel(+200.0)); - // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetTurretAbs(+0.0)); + DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new TurretSetRel(-200.0)); + DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new TurretSetRel(+200.0)); + DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new TurretSetAbs(+0.0)); // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new // ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); @@ -200,14 +201,15 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0)); DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); - DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new ShooterCloseFiringSequence(60.0)); - DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); + // DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new + // ShooterCloseFiringSequence(60.0)); + // DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new + // ShooterCeaseFire()); DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whenPressed(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 240, 0)); DRIVER_PAD.DRIVER_PAD_START_BUTTON.whenPressed(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 240, 0)); - // DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new - // FeederSet(1.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0)); } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java new file mode 100644 index 0000000..b95dafc --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.*; + +public class DriveSlalom extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public DriveSlalom() { + // wut + addCommands(new DriveZeroGyro(0.0)); + + // drive forward + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 11, 0)); + // turn + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 295)); + // drive forward some more + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 11, 0)); + // turn again + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 65)); + // forward + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 48, 0)); + // turn again again + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 54, 270)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 54, 185)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 60, 115)); + + // lotta driving + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 11.5, 180)); + // turn again again 2 electic boogalo + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 245)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 36, 180)); + } +} From 70fd2ea52c068f6994164ee1bdc3f03dd821dff1 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Fri, 15 Jan 2021 23:09:12 -0500 Subject: [PATCH 099/121] Add PathWeaver slalom path --- PathWeaver/Paths/Unnamed.path | 3 --- PathWeaver/Paths/Unnamed_0.path | 3 --- PathWeaver/Paths/slalom.path | 14 ++++++++++++++ PathWeaver/pathweaver.json | 6 +++--- 4 files changed, 17 insertions(+), 9 deletions(-) delete mode 100644 PathWeaver/Paths/Unnamed.path delete mode 100644 PathWeaver/Paths/Unnamed_0.path create mode 100644 PathWeaver/Paths/slalom.path diff --git a/PathWeaver/Paths/Unnamed.path b/PathWeaver/Paths/Unnamed.path deleted file mode 100644 index 93d59e1..0000000 --- a/PathWeaver/Paths/Unnamed.path +++ /dev/null @@ -1,3 +0,0 @@ -X,Y,Tangent X,Tangent Y,Fixed Theta,Name -0.0,0.0,120.0,0.0,true, -120.0,-120.0,0.0,-120.0,true, diff --git a/PathWeaver/Paths/Unnamed_0.path b/PathWeaver/Paths/Unnamed_0.path deleted file mode 100644 index 93d59e1..0000000 --- a/PathWeaver/Paths/Unnamed_0.path +++ /dev/null @@ -1,3 +0,0 @@ -X,Y,Tangent X,Tangent Y,Fixed Theta,Name -0.0,0.0,120.0,0.0,true, -120.0,-120.0,0.0,-120.0,true, diff --git a/PathWeaver/Paths/slalom.path b/PathWeaver/Paths/slalom.path new file mode 100644 index 0000000..1589582 --- /dev/null +++ b/PathWeaver/Paths/slalom.path @@ -0,0 +1,14 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +1.261042710938696,-3.894811444030423,2.1092932851819723,0.02981333265274877,true,false, +2.595189347149202,-2.918424799652902,0.23105332805880252,0.4173866571384819,true,false, +3.4895893267316636,-1.956944821601756,0.9208748621429468,0.5341361651409312,false,false, +5.606335945076822,-1.9793048210913178,2.072026619366035,-0.9242133122352103,true,false, +6.865949249655455,-3.3581714562809455,0.3875733244857331,-0.864586646929713,true,false, +7.752895896074729,-3.909718110356797,1.050919976009392,-0.022359999489561577,true,false, +8.774002539431374,-3.0227714639375236,-0.18633332907968025,1.341599969373693,true,false, +7.849789227196162,-2.038931486396815,-1.1627199734571994,0.03726666581593596,true,false, +6.716882586391711,-3.0227714639375236,-0.30558665969067356,-1.5726532974324945,true,false, +5.747949275177379,-3.961891442499107,-1.140359973967639,-0.2981333265274868,true,false, +3.422509328262978,-3.917171443519984,-0.6782533178500332,0.2683199938747385,true,false, +2.0510960262365376,-2.79171813587872,-0.9465733117247719,1.6844532948803024,true,false, +0.5529760604359146,-2.1730914833341846,-0.8422266474401512,-0.03726666581593552,true,false, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json index 5fe7ade..4481449 100644 --- a/PathWeaver/pathweaver.json +++ b/PathWeaver/pathweaver.json @@ -1,9 +1,9 @@ { - "lengthUnit": "Inch", + "lengthUnit": "Meter", "exportUnit": "Always Meters", "maxVelocity": 5.0, "maxAcceleration": 5.0, "wheelBase": 5.0, - "gameName": "Infinite Recharge", + "gameName": "Slalom Path", "outputDir": "" -} \ No newline at end of file +} From d4cccaed6a31c3e4e06ad7727345aea42767d147 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 11 Feb 2021 22:17:01 -0500 Subject: [PATCH 100/121] Cumulative changes to robot code during late Jan 2021 and early Feb 2021: added DriveTest and TurnToHeading routines, added cancelAll() to teleopInit(), various attempts at debugging some odd issues with the new "TurnToHeading" routine. --- src/main/java/frc/robot/Robot.java | 1 + .../org/mayheminc/robot2020/Constants.java | 11 +- .../mayheminc/robot2020/RobotContainer.java | 4 + .../autonomousroutines/DriveSlalom.java | 2 +- .../autonomousroutines/DriveTest.java | 48 ++++++++ .../robot2020/commands/DriveDefault.java | 2 + .../robot2020/commands/TurnToHeading.java | 103 ++++++++++++++++++ .../mayheminc/robot2020/subsystems/Drive.java | 26 +++-- 8 files changed, 183 insertions(+), 14 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java create mode 100644 src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0aac71e..7b8fa48 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -69,6 +69,7 @@ public void robotPeriodic() { @Override public void disabledInit() { RobotContainer.init(); + CommandScheduler.getInstance().cancelAll(); } @Override diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 3362fd9..9eebfc7 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -42,7 +42,7 @@ public final class Talon { public static final int CLIMBER_WALKER_LEFT = 15; public static final int CLIMBER_WALKER_RIGHT = 16; - public static final int CONTROL_PANEL_ROLLER = 18; // WON"T FIT!!! + public static final int CONTROL_PANEL_ROLLER = 18; // WON'T FIT!!! } public final class Solenoid { @@ -61,4 +61,13 @@ public final class PDP { public static final int DRIVE_LEFT_B = 4; } + public final class Drive { + public static final int DRIVEBASE_WIDTH = 25; + + /** + * A correction applied to the radius of arcing turns (inches) + */ + public static final double TURNING_CORRECTION = 1.84; + } + } diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 074f584..4d04861 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -12,8 +12,11 @@ // import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.Relay.Direction; import edu.wpi.first.wpilibj2.command.Command; // import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import java.util.LinkedList; @@ -102,6 +105,7 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); + autonomousPrograms.push(/* 14 */ new DriveTest()); autonomousPrograms.push(/* 13 */ new DriveSlalom()); autonomousPrograms.push(/* 12 */ new StayStill()); autonomousPrograms.push(/* 11 */ new StartBWDriveOnlyToRP()); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java index b95dafc..ee1875e 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java @@ -37,7 +37,7 @@ public DriveSlalom() { addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 60, 115)); // lotta driving - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 11.5, 180)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 12, 180)); // turn again again 2 electic boogalo addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 245)); addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 36, 180)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java new file mode 100644 index 0000000..3ffc8c6 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.*; + +public class DriveTest extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public DriveTest() { + // wut + addCommands(new DriveZeroGyro(0.0)); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, 0)); + // addCommands(new Wait(0.02)); + // test stuff + addCommands(new TurnToHeading(25, 0.4, 70, TurnToHeading.Direction.RIGHT)); + // turn + // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, 90)); + // // addCommands(new Wait(0.03)); + + // // test stuff + // addCommands(new TurnToHeading(25, 0.4, 160, TurnToHeading.Direction.RIGHT)); + // // turn + // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, 180)); + // // addCommands(new Wait(0.03)); + // // test stuff + // addCommands(new TurnToHeading(25, 0.4, -110, TurnToHeading.Direction.RIGHT)); + // // turn + // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, -90)); + // // addCommands(new Wait(0.03)); + // // test stuff + // addCommands(new TurnToHeading(25, 0.4, -20, TurnToHeading.Direction.RIGHT)); + // // turn + // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, 0)); + // // addCommands(new Wait(0.03)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java b/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java index c884865..4bffe97 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java @@ -27,6 +27,8 @@ public void execute() { double steeringX = RobotContainer.DRIVER_PAD.steeringX(); boolean quickTurn = RobotContainer.DRIVER_PAD.quickTurn(); RobotContainer.drive.speedRacerDrive(throttle, steeringX, quickTurn); + // RobotContainer.drive.speedRacerDrive(0, 0, false); + } // Returns true when the command should end. diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java new file mode 100644 index 0000000..160f127 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java @@ -0,0 +1,103 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.subsystems.Drive; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurnToHeading extends CommandBase { + double distanceFromPoint; + double speed; + double exitHeading; + double initialHeading; + Direction direction; + int turnPhase; + + public enum Direction { + LEFT, RIGHT + } + + /** + * Creates a new TurnToHeading. + */ + public TurnToHeading(double distanceFromPoint, double speed, double exitHeading, Direction direction) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.drive); + + this.turnPhase = 0; + this.distanceFromPoint = distanceFromPoint; + this.speed = speed; + this.exitHeading = exitHeading; + this.direction = direction; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.initialHeading = RobotContainer.drive.getHeading(); + this.turnPhase = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (direction == Direction.LEFT) { + RobotContainer.drive.tankDrive(speed * getRatio(), speed); + } else { + RobotContainer.drive.tankDrive(speed, speed * getRatio()); + } + } + + public double getRatio() { + return distanceFromPoint / (distanceFromPoint + Constants.Drive.DRIVEBASE_WIDTH) + / Constants.Drive.TURNING_CORRECTION; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.drive.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (direction == Direction.LEFT) { + if (initialHeading < exitHeading) { + if (turnPhase == 0) { + if (RobotContainer.drive.getHeading() > (initialHeading + 25)) { + turnPhase = 1; + } + return false; + } else { // turnPhase == 1 + return (RobotContainer.drive.getHeading() < exitHeading); + } + } else { + return (RobotContainer.drive.getHeading() < exitHeading + || RobotContainer.drive.getHeading() > (initialHeading + 25)); + } + } else {// direction == right + if (initialHeading > exitHeading) { + if (turnPhase == 0) { + if (RobotContainer.drive.getHeading() < (initialHeading - 25)) { + turnPhase = 1; + } + return false; + } else { // turnPhase == 1 + return (RobotContainer.drive.getHeading() > exitHeading); + } + } else { + return (RobotContainer.drive.getHeading() > exitHeading + || RobotContainer.drive.getHeading() < (initialHeading - 25)); + } + } + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index d9560bc..0bb1d0f 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -387,18 +387,20 @@ public void updateSdbPdp() { * the right side of the joystick controls the right side of the robot. */ public void tankDrive(double leftSideThrottle, double rightSideThrottle) { - if (leftSideThrottle >= 0.0) { - leftSideThrottle = (leftSideThrottle * leftSideThrottle); // squaring inputs increases fine control - } else { - leftSideThrottle = -(leftSideThrottle * leftSideThrottle); // preserves the sign while squaring negative - // values - } - - if (rightSideThrottle >= 0.0) { - rightSideThrottle = (rightSideThrottle * rightSideThrottle); - } else { - rightSideThrottle = -(rightSideThrottle * rightSideThrottle); - } + // if (leftSideThrottle >= 0.0) { + // leftSideThrottle = (leftSideThrottle * leftSideThrottle); // squaring inputs + // increases fine control + // } else { + // leftSideThrottle = -(leftSideThrottle * leftSideThrottle); // preserves the + // sign while squaring negative + // // values + // } + + // if (rightSideThrottle >= 0.0) { + // rightSideThrottle = (rightSideThrottle * rightSideThrottle); + // } else { + // rightSideThrottle = -(rightSideThrottle * rightSideThrottle); + // } setMotorPower(leftSideThrottle, rightSideThrottle); } From a2e876fb8f4ed2fc379fcf1eb8f5641ff9e39c12 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 11 Feb 2021 22:20:59 -0500 Subject: [PATCH 101/121] Cleaned up various warnings in the code from unused imports. --- src/main/java/org/mayheminc/robot2020/RobotContainer.java | 5 ----- .../mayheminc/robot2020/autonomousroutines/DriveSlalom.java | 1 - .../mayheminc/robot2020/autonomousroutines/DriveTest.java | 1 - .../java/org/mayheminc/robot2020/commands/TurnToHeading.java | 1 - 4 files changed, 8 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 4d04861..803a472 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -9,14 +9,9 @@ import org.mayheminc.util.*; -// import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj.Relay.Direction; import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj2.command.button.Button; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import java.util.LinkedList; diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java index ee1875e..b24966c 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java @@ -9,7 +9,6 @@ import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; -import org.mayheminc.robot2020.subsystems.Intake; import edu.wpi.first.wpilibj2.command.*; diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java index 3ffc8c6..0f3a53b 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java @@ -9,7 +9,6 @@ import org.mayheminc.robot2020.commands.*; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; -import org.mayheminc.robot2020.subsystems.Intake; import edu.wpi.first.wpilibj2.command.*; diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java index 160f127..1569be8 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java @@ -9,7 +9,6 @@ import org.mayheminc.robot2020.Constants; import org.mayheminc.robot2020.RobotContainer; -import org.mayheminc.robot2020.subsystems.Drive; import edu.wpi.first.wpilibj2.command.CommandBase; From 9c65660ada9807d1e1582e327efdc5ac71e81f9e Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Tue, 16 Feb 2021 21:08:05 -0500 Subject: [PATCH 102/121] Fix TurnToHeading --- .../org/mayheminc/robot2020/Constants.java | 2 +- .../autonomousroutines/DriveTest.java | 53 ++++++++++--------- 2 files changed, 30 insertions(+), 25 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 9eebfc7..9c9bf17 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -67,7 +67,7 @@ public final class Drive { /** * A correction applied to the radius of arcing turns (inches) */ - public static final double TURNING_CORRECTION = 1.84; + public static final double TURNING_CORRECTION = 1.00; } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java index 0f3a53b..5ca4740 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java @@ -17,31 +17,36 @@ public class DriveTest extends SequentialCommandGroup { * Add your docs here. */ public DriveTest() { - // wut addCommands(new DriveZeroGyro(0.0)); - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, 0)); - // addCommands(new Wait(0.02)); - // test stuff - addCommands(new TurnToHeading(25, 0.4, 70, TurnToHeading.Direction.RIGHT)); - // turn - // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, 90)); - // // addCommands(new Wait(0.03)); - - // // test stuff - // addCommands(new TurnToHeading(25, 0.4, 160, TurnToHeading.Direction.RIGHT)); - // // turn - // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, 180)); - // // addCommands(new Wait(0.03)); - // // test stuff - // addCommands(new TurnToHeading(25, 0.4, -110, TurnToHeading.Direction.RIGHT)); - // // turn - // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, -90)); - // // addCommands(new Wait(0.03)); - // // test stuff - // addCommands(new TurnToHeading(25, 0.4, -20, TurnToHeading.Direction.RIGHT)); - // // turn - // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 36, 0)); - // // addCommands(new Wait(0.03)); + // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 0)); + // addCommands(new TurnToHeading(25, 0.25, 45, TurnToHeading.Direction.RIGHT)); + + // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 45)); + // addCommands(new TurnToHeading(25, 0.25, 135, TurnToHeading.Direction.RIGHT)); + + // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 135)); + // addCommands(new TurnToHeading(25, 0.25, -135, + // TurnToHeading.Direction.RIGHT)); + + // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, + // -135)); + // addCommands(new TurnToHeading(25, 0.25, -45, TurnToHeading.Direction.RIGHT)); + + // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, -45)); + + addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 0)); + addCommands(new TurnToHeading(25, 0.25, -45, TurnToHeading.Direction.LEFT)); + + addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, -45)); + addCommands(new TurnToHeading(25, 0.25, -135, TurnToHeading.Direction.LEFT)); + + addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, -135)); + addCommands(new TurnToHeading(25, 0.25, 135, TurnToHeading.Direction.LEFT)); + + addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 135)); + addCommands(new TurnToHeading(25, 0.25, 45, TurnToHeading.Direction.LEFT)); + + addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 45)); } } From f6995415f0262680b31380ef84210e3b84ebdb18 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Tue, 16 Feb 2021 21:39:22 -0500 Subject: [PATCH 103/121] Migrate DriveSlalom to TurnToHeading --- .../autonomousroutines/DriveSlalom.java | 51 +++++++++++-------- 1 file changed, 31 insertions(+), 20 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java index b24966c..fd8202f 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java @@ -17,28 +17,39 @@ public class DriveSlalom extends SequentialCommandGroup { * Add your docs here. */ public DriveSlalom() { - // wut addCommands(new DriveZeroGyro(0.0)); - // drive forward - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 11, 0)); - // turn - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 295)); - // drive forward some more - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 11, 0)); - // turn again - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 65)); - // forward - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 48, 0)); - // turn again again - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 54, 270)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 54, 185)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 60, 115)); + // // drive forward + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 11, 0)); + // // turn + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 295)); + // // drive forward some more + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 11, + // 0)); + // // turn again + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 65)); + // // forward + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 48, 0)); + // // turn again again + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 54, 270)); + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 54, 185)); + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 60, 115)); - // lotta driving - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 12, 180)); - // turn again again 2 electic boogalo - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 245)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 36, 180)); + // // lotta driving + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 12, + // 180)); + // // turn again again 2 electic boogalo + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 245)); + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 36, 180)); + + addCommands(new TurnToHeading(20, 0.3, -45, TurnToHeading.Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 15, 295)); + addCommands(new TurnToHeading(20, 0.3, 0, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0)); + addCommands(new TurnToHeading(20, 0.3, 45, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 20, 45)); + addCommands(new WaitCommand(2)); + addCommands(new TurnToHeading(0, 0.3, -45, TurnToHeading.Direction.LEFT)); + addCommands(new TurnToHeading(0, 0.3, -135, TurnToHeading.Direction.LEFT)); } } From f3f842734f70c38af8257eebf4048abdd8dd1c57 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Tue, 16 Feb 2021 22:50:28 -0500 Subject: [PATCH 104/121] Remove pull_request CI --- .github/workflows/build.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index c009cae..ae19f6c 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,7 +1,6 @@ name: Build on: - push - - pull_request jobs: build: name: Build @@ -10,5 +9,5 @@ jobs: - uses: actions/checkout@v2 - uses: actions/setup-java@v1 with: - java-version: '11.0.2' + java-version: "11.0.2" - run: ./gradlew build From df48f57e8bbc6d4022070859051fa43e89847867 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Mon, 22 Feb 2021 16:39:24 -0500 Subject: [PATCH 105/121] Fix TurnToHeading and work on DriveSlalom --- .../autonomousroutines/DriveSlalom.java | 22 +++++++++++++------ .../robot2020/commands/TurnToHeading.java | 15 ++++++++----- 2 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java index fd8202f..fe88b28 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java @@ -44,12 +44,20 @@ public DriveSlalom() { addCommands(new TurnToHeading(20, 0.3, -45, TurnToHeading.Direction.LEFT)); addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 15, 295)); - addCommands(new TurnToHeading(20, 0.3, 0, TurnToHeading.Direction.RIGHT)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 120, 0)); - addCommands(new TurnToHeading(20, 0.3, 45, TurnToHeading.Direction.RIGHT)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 20, 45)); - addCommands(new WaitCommand(2)); - addCommands(new TurnToHeading(0, 0.3, -45, TurnToHeading.Direction.LEFT)); - addCommands(new TurnToHeading(0, 0.3, -135, TurnToHeading.Direction.LEFT)); + addCommands(new TurnToHeading(20, 0.4, 0, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 66, 0)); + addCommands(new TurnToHeading(20, 0.3, 50, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 20, 50)); + // COOL TURN ⬇️ + addCommands(new TurnToHeading(1, 0.42, -45, TurnToHeading.Direction.LEFT)); + addCommands(new TurnToHeading(1, 0.42, -135, TurnToHeading.Direction.LEFT)); + addCommands(new TurnToHeading(1, 0.42, 120, TurnToHeading.Direction.LEFT)); + // COOL TURN ⬆️ + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 30, 135)); + addCommands(new TurnToHeading(1, 0.3, -180, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 115, 180)); + addCommands(new TurnToHeading(1, 0.3, -135, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 56, -135)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 15, -180)); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java index 1569be8..5f731db 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java @@ -20,6 +20,8 @@ public class TurnToHeading extends CommandBase { Direction direction; int turnPhase; + double ratio; + public enum Direction { LEFT, RIGHT } @@ -43,16 +45,19 @@ public TurnToHeading(double distanceFromPoint, double speed, double exitHeading, public void initialize() { this.initialHeading = RobotContainer.drive.getHeading(); this.turnPhase = 0; + + this.ratio = getRatio(); + + if (direction == Direction.LEFT) { + RobotContainer.drive.tankDrive(speed * ratio, speed); + } else { + RobotContainer.drive.tankDrive(speed, speed * ratio); + } } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (direction == Direction.LEFT) { - RobotContainer.drive.tankDrive(speed * getRatio(), speed); - } else { - RobotContainer.drive.tankDrive(speed, speed * getRatio()); - } } public double getRatio() { From 626c1ee6e016dd6600f547c65c2d1ac79a1c29b0 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Thu, 25 Feb 2021 19:58:42 -0500 Subject: [PATCH 106/121] Start writing barrel racing auto --- .../mayheminc/robot2020/RobotContainer.java | 3 +- .../autonomousroutines/DriveBarrelRacing.java | 42 +++++++++++++++++++ 2 files changed, 44 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 803a472..026541b 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -100,7 +100,8 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - autonomousPrograms.push(/* 14 */ new DriveTest()); + autonomousPrograms.push(/* 15 */ new DriveTest()); + autonomousPrograms.push(/* 14 */ new DriveBarrelRacing()); autonomousPrograms.push(/* 13 */ new DriveSlalom()); autonomousPrograms.push(/* 12 */ new StayStill()); autonomousPrograms.push(/* 11 */ new StartBWDriveOnlyToRP()); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java new file mode 100644 index 0000000..569ecb2 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.TurnToHeading; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.commands.TurnToHeading.Direction; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DriveBarrelRacing extends SequentialCommandGroup { + /** Creates a new DriveBarrelRacing. */ + public DriveBarrelRacing() { + // TODO: make work on actual robot + + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new DriveZeroGyro(0.0)); + + addCommands(new DriveStraightOnHeading(0.4, 102, 0)); + + addCommands(new TurnToHeading(12, 0.4, 180, Direction.RIGHT)); + addCommands(new TurnToHeading(12, 0.4, -30, Direction.RIGHT)); + + addCommands(new DriveStraightOnHeading(0.4, 94, 0)); + + addCommands(new TurnToHeading(12, 0.4, 180, Direction.LEFT)); + addCommands(new TurnToHeading(12, 0.4, 50, Direction.LEFT)); + + addCommands(new DriveStraightOnHeading(0.4, 60, 50)); + + addCommands(new TurnToHeading(12, 0.4, 180, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.4, 20 * 12, 180)); + } +} From 8df68117fdb646641627960ee6d87d1a37e83c27 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Thu, 25 Feb 2021 21:11:31 -0500 Subject: [PATCH 107/121] Start bounce path and make TurnToHeading work in reverse --- .../mayheminc/robot2020/RobotContainer.java | 3 +- .../autonomousroutines/DriveBouncePath.java | 38 +++++++++++++++++++ .../robot2020/commands/TurnToHeading.java | 4 +- 3 files changed, 42 insertions(+), 3 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 026541b..5ba5daf 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -100,7 +100,8 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - autonomousPrograms.push(/* 15 */ new DriveTest()); + autonomousPrograms.push(/* 16 */ new DriveTest()); + autonomousPrograms.push(/* 15 */ new DriveBouncePath()); autonomousPrograms.push(/* 14 */ new DriveBarrelRacing()); autonomousPrograms.push(/* 13 */ new DriveSlalom()); autonomousPrograms.push(/* 12 */ new StayStill()); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java new file mode 100644 index 0000000..fd1b1e6 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.TurnToHeading; + +import org.mayheminc.robot2020.commands.TurnToHeading.Direction; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DriveBouncePath extends SequentialCommandGroup { + /** Creates a new DriveBouncePath. */ + public DriveBouncePath() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new DriveZeroGyro(0.0)); + addCommands(new DriveStraightOnHeading(0.4, 18, 0)); + addCommands(new TurnToHeading(12, 0.4, -90, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.4, 6, -90)); + addCommands(new DriveStraightOnHeading(-0.4, 6.5 * 12, -114)); + addCommands(new TurnToHeading(12, -0.4, 100, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(-0.4, 5 * 12, 90)); + addCommands(new DriveStraightOnHeading(0.4, 5.5 * 12, 90)); + addCommands(new TurnToHeading(12, 0.4, 0, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.4, 1.5 * 12, 0)); + addCommands(new TurnToHeading(12, 0.4, -80, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.4, 5 * 12, -90)); + addCommands(new DriveStraightOnHeading(-0.4, 12, -90)); + addCommands(new TurnToHeading(12, -0.4, -180, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(-0.4, 12, -180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java index 5f731db..f647424 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java @@ -48,9 +48,9 @@ public void initialize() { this.ratio = getRatio(); - if (direction == Direction.LEFT) { + if ((speed > 0 && direction == Direction.LEFT) || (speed < 0 && direction == Direction.RIGHT)) { RobotContainer.drive.tankDrive(speed * ratio, speed); - } else { + } else if ((speed > 0 && direction == Direction.RIGHT) || (speed < 0 && direction == Direction.LEFT)) { RobotContainer.drive.tankDrive(speed, speed * ratio); } } From 053a041f1a9d4d75cf91e803989ba4625a19086f Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Mon, 1 Mar 2021 21:45:36 -0500 Subject: [PATCH 108/121] Make bounce path and barrel racing work on actual robot --- .../autonomousroutines/DriveBarrelRacing.java | 13 ++++--- .../autonomousroutines/DriveBouncePath.java | 35 ++++++++++++------- .../robot2020/commands/TurnToHeading.java | 4 +++ 3 files changed, 35 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java index 569ecb2..1339d46 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java @@ -6,9 +6,11 @@ import org.mayheminc.robot2020.commands.DriveStraightOnHeading; import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; import org.mayheminc.robot2020.commands.TurnToHeading; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; import org.mayheminc.robot2020.commands.TurnToHeading.Direction; +import org.mayheminc.robot2020.subsystems.Intake; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -23,20 +25,21 @@ public DriveBarrelRacing() { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands(new DriveZeroGyro(0.0)); + addCommands(new IntakeSetPosition(Intake.PIVOT_UP)); addCommands(new DriveStraightOnHeading(0.4, 102, 0)); - addCommands(new TurnToHeading(12, 0.4, 180, Direction.RIGHT)); - addCommands(new TurnToHeading(12, 0.4, -30, Direction.RIGHT)); + addCommands(new TurnToHeading(1, 0.4, 180, Direction.RIGHT)); + addCommands(new TurnToHeading(1, 0.4, -30, Direction.RIGHT)); addCommands(new DriveStraightOnHeading(0.4, 94, 0)); - addCommands(new TurnToHeading(12, 0.4, 180, Direction.LEFT)); - addCommands(new TurnToHeading(12, 0.4, 50, Direction.LEFT)); + addCommands(new TurnToHeading(1, 0.4, 180, Direction.LEFT)); + addCommands(new TurnToHeading(1, 0.4, 50, Direction.LEFT)); addCommands(new DriveStraightOnHeading(0.4, 60, 50)); - addCommands(new TurnToHeading(12, 0.4, 180, Direction.LEFT)); + addCommands(new TurnToHeading(1, 0.4, 180, Direction.LEFT)); addCommands(new DriveStraightOnHeading(0.4, 20 * 12, 180)); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java index fd1b1e6..3700a30 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java @@ -6,9 +6,12 @@ import org.mayheminc.robot2020.commands.DriveStraightOnHeading; import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; import org.mayheminc.robot2020.commands.TurnToHeading; import org.mayheminc.robot2020.commands.TurnToHeading.Direction; +import org.mayheminc.robot2020.subsystems.Intake; + import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -20,19 +23,27 @@ public DriveBouncePath() { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands(new DriveZeroGyro(0.0)); - addCommands(new DriveStraightOnHeading(0.4, 18, 0)); - addCommands(new TurnToHeading(12, 0.4, -90, Direction.LEFT)); + addCommands(new IntakeSetPosition(Intake.PIVOT_UP)); + + addCommands(new DriveStraightOnHeading(0.4, 12, 0)); + addCommands(new TurnToHeading(1, 0.4, -90, Direction.LEFT)); addCommands(new DriveStraightOnHeading(0.4, 6, -90)); - addCommands(new DriveStraightOnHeading(-0.4, 6.5 * 12, -114)); - addCommands(new TurnToHeading(12, -0.4, 100, Direction.LEFT)); - addCommands(new DriveStraightOnHeading(-0.4, 5 * 12, 90)); - addCommands(new DriveStraightOnHeading(0.4, 5.5 * 12, 90)); - addCommands(new TurnToHeading(12, 0.4, 0, Direction.LEFT)); + + // just hit first ball, now back away + addCommands(new DriveStraightOnHeading(-0.4, 6.5 * 12, -116)); + addCommands(new TurnToHeading(1, -0.4, 100, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(-0.4, 6.5 * 12, 90)); + + // just hit second ball, now back away + addCommands(new DriveStraightOnHeading(0.4, 5.1 * 12, 90)); + addCommands(new TurnToHeading(1, 0.4, 0, Direction.LEFT)); addCommands(new DriveStraightOnHeading(0.4, 1.5 * 12, 0)); - addCommands(new TurnToHeading(12, 0.4, -80, Direction.LEFT)); - addCommands(new DriveStraightOnHeading(0.4, 5 * 12, -90)); - addCommands(new DriveStraightOnHeading(-0.4, 12, -90)); - addCommands(new TurnToHeading(12, -0.4, -180, Direction.LEFT)); - addCommands(new DriveStraightOnHeading(-0.4, 12, -180)); + addCommands(new TurnToHeading(1, 0.4, -80, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.4, 6 * 12, -90)); + + // just hit third ball, now back away + addCommands(new DriveStraightOnHeading(-0.4, 6, -90)); + addCommands(new TurnToHeading(1, -0.4, -160, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(-0.4, 0.5 * 12, -180)); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java index f647424..13d6bec 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java @@ -50,8 +50,12 @@ public void initialize() { if ((speed > 0 && direction == Direction.LEFT) || (speed < 0 && direction == Direction.RIGHT)) { RobotContainer.drive.tankDrive(speed * ratio, speed); + System.out.println("LEFT: " + speed * ratio); + System.out.println("RIGHT: " + speed); } else if ((speed > 0 && direction == Direction.RIGHT) || (speed < 0 && direction == Direction.LEFT)) { RobotContainer.drive.tankDrive(speed, speed * ratio); + System.out.println("LEFT: " + speed); + System.out.println("RIGHT: " + speed * ratio); } } From dd53c1623950fc5500041e7398faee8d22a28518 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Thu, 4 Mar 2021 12:47:28 -0500 Subject: [PATCH 109/121] Increased speed of robot in DriveBarrelRacing.java to be 60% of maximum speed. Added a "PrintAutonomousTimeElapsed" command to provide better timing of programs. --- src/main/java/frc/robot/Robot.java | 8 +++++ .../mayheminc/robot2020/RobotContainer.java | 3 ++ .../autonomousroutines/DriveBarrelRacing.java | 22 +++++++------ .../autonomousroutines/DriveTest.java | 30 ++---------------- .../commands/PrintAutonomousTimeElapsed.java | 31 +++++++++++++++++++ 5 files changed, 57 insertions(+), 37 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7b8fa48..009c3f4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,6 +28,8 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; + public static double autonomousStartTime = -1; + /** * This function is run when the robot is first started up and should be used * for any initialization code. @@ -91,6 +93,8 @@ public void autonomousInit() { if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } + + autonomousStartTime = System.currentTimeMillis(); } /** @@ -135,4 +139,8 @@ public void testInit() { @Override public void testPeriodic() { } + + public static double autonomousTimeElapsed() { + return (System.currentTimeMillis() - autonomousStartTime) / 1000; + } } diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 5ba5daf..d17990a 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -9,6 +9,9 @@ import org.mayheminc.util.*; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.HALUtil; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java index 1339d46..f26f28c 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java @@ -7,11 +7,13 @@ import org.mayheminc.robot2020.commands.DriveStraightOnHeading; import org.mayheminc.robot2020.commands.DriveZeroGyro; import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; import org.mayheminc.robot2020.commands.TurnToHeading; import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; import org.mayheminc.robot2020.commands.TurnToHeading.Direction; import org.mayheminc.robot2020.subsystems.Intake; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -27,19 +29,21 @@ public DriveBarrelRacing() { addCommands(new DriveZeroGyro(0.0)); addCommands(new IntakeSetPosition(Intake.PIVOT_UP)); - addCommands(new DriveStraightOnHeading(0.4, 102, 0)); + addCommands(new DriveStraightOnHeading(0.6, 80, 0)); - addCommands(new TurnToHeading(1, 0.4, 180, Direction.RIGHT)); - addCommands(new TurnToHeading(1, 0.4, -30, Direction.RIGHT)); + addCommands(new TurnToHeading(1.5, 0.6, 180, Direction.RIGHT)); + addCommands(new TurnToHeading(1.5, 0.6, -55, Direction.RIGHT)); - addCommands(new DriveStraightOnHeading(0.4, 94, 0)); + addCommands(new DriveStraightOnHeading(0.6, 90, -12)); - addCommands(new TurnToHeading(1, 0.4, 180, Direction.LEFT)); - addCommands(new TurnToHeading(1, 0.4, 50, Direction.LEFT)); + addCommands(new TurnToHeading(1.5, 0.6, 180, Direction.LEFT)); + addCommands(new TurnToHeading(1.5, 0.6, 80, Direction.LEFT)); - addCommands(new DriveStraightOnHeading(0.4, 60, 50)); + addCommands(new DriveStraightOnHeading(0.6, 70, 60)); - addCommands(new TurnToHeading(1, 0.4, 180, Direction.LEFT)); - addCommands(new DriveStraightOnHeading(0.4, 20 * 12, 180)); + addCommands(new TurnToHeading(1.5, 0.6, -160, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.6, 20 * 12, -178)); + + addCommands(new PrintAutonomousTimeElapsed("BarrelRacing")); } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java index 5ca4740..10d36b6 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java @@ -19,34 +19,8 @@ public class DriveTest extends SequentialCommandGroup { public DriveTest() { addCommands(new DriveZeroGyro(0.0)); - // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 0)); - // addCommands(new TurnToHeading(25, 0.25, 45, TurnToHeading.Direction.RIGHT)); + addCommands(new WaitCommand(5)); - // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 45)); - // addCommands(new TurnToHeading(25, 0.25, 135, TurnToHeading.Direction.RIGHT)); - - // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 135)); - // addCommands(new TurnToHeading(25, 0.25, -135, - // TurnToHeading.Direction.RIGHT)); - - // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, - // -135)); - // addCommands(new TurnToHeading(25, 0.25, -45, TurnToHeading.Direction.RIGHT)); - - // addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, -45)); - - addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 0)); - addCommands(new TurnToHeading(25, 0.25, -45, TurnToHeading.Direction.LEFT)); - - addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, -45)); - addCommands(new TurnToHeading(25, 0.25, -135, TurnToHeading.Direction.LEFT)); - - addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, -135)); - addCommands(new TurnToHeading(25, 0.25, 135, TurnToHeading.Direction.LEFT)); - - addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 135)); - addCommands(new TurnToHeading(25, 0.25, 45, TurnToHeading.Direction.LEFT)); - - addCommands(new DriveStraightOnHeading(0.25, DistanceUnits.INCHES, 36, 45)); + addCommands(new PrintAutonomousTimeElapsed("yay")); } } diff --git a/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java new file mode 100644 index 0000000..50a8823 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java @@ -0,0 +1,31 @@ +package org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj.DriverStation; + +import org.mayheminc.robot2020.RobotContainer; +import frc.robot.Robot; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class PrintAutonomousTimeElapsed extends CommandBase { + String Message = ""; + + public PrintAutonomousTimeElapsed(String msg) { + this.Message = msg; + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + DriverStation.reportError(Message + " At: " + Robot.autonomousTimeElapsed() + "\n", false); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return true; + } +} From 0e0b2ce6253f9bf6f147a714b3ef959e9129b538 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Mon, 8 Mar 2021 21:49:35 -0500 Subject: [PATCH 110/121] Add galatic search paths and tweak intake PID --- .../mayheminc/robot2020/RobotContainer.java | 10 ++--- .../autonomousroutines/PathABlue.java | 45 +++++++++++++++++++ .../autonomousroutines/PathARed.java | 45 +++++++++++++++++++ .../autonomousroutines/PathBBlue.java | 44 ++++++++++++++++++ .../commands/PrintAutonomousTimeElapsed.java | 1 - .../robot2020/subsystems/Intake.java | 6 +-- 6 files changed, 142 insertions(+), 9 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBBlue.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index d17990a..7caddb9 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -9,9 +9,6 @@ import org.mayheminc.util.*; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.hal.HALUtil; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj2.command.Command; @@ -67,7 +64,7 @@ public RobotContainer() { pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN, RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN, - RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, turret); + RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, intake); cameraLights.set(true); } @@ -103,7 +100,10 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - autonomousPrograms.push(/* 16 */ new DriveTest()); + autonomousPrograms.push(/* 19 */ new DriveTest()); + autonomousPrograms.push(/* 18 */ new PathARed()); + autonomousPrograms.push(/* 17 */ new PathBBlue()); + autonomousPrograms.push(/* 16 */ new PathABlue()); autonomousPrograms.push(/* 15 */ new DriveBouncePath()); autonomousPrograms.push(/* 14 */ new DriveBarrelRacing()); autonomousPrograms.push(/* 13 */ new DriveSlalom()); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java new file mode 100644 index 0000000..c6a02ed --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.IntakeSetRollers; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class PathABlue extends SequentialCommandGroup { + /** + * Creates a new PathABlue. + */ + public PathABlue() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands(new DriveZeroGyro(0.0)); + + addCommands(new IntakeSetRollers(-1)); + + addCommands(new ParallelCommandGroup(new IntakeSetPosition(Intake.PIVOT_DOWN), + new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 10 * 12, 0))); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 7.5 * 12, -85)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 13 * 12, 50)); + + addCommands(new IntakeSetRollers(0)); + addCommands(new PrintAutonomousTimeElapsed("Path A Blue")); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java new file mode 100644 index 0000000..3e53320 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.IntakeSetRollers; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Drive; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class PathARed extends SequentialCommandGroup { + /** + * Creates a new PathABlue. + */ + public PathARed() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands(new DriveZeroGyro(26.0)); + + addCommands(new IntakeSetRollers(-1)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.5 * 12, 26.0)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.8 * 12, -95)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 14 * 12, 0)); + + addCommands(new IntakeSetRollers(0)); + addCommands(new PrintAutonomousTimeElapsed("Path A Red")); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBBlue.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBBlue.java new file mode 100644 index 0000000..ddae266 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBBlue.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.IntakeSetRollers; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class PathBBlue extends SequentialCommandGroup { + /** + * Creates a new PathABlue. + */ + public PathBBlue() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands(new DriveZeroGyro(-8)); + + addCommands(new IntakeSetRollers(-1)); + + addCommands(new ParallelCommandGroup(new IntakeSetPosition(Intake.PIVOT_DOWN), + new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 10 * 12, -8))); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 5.5 * 12, -55)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 12 * 12, 55)); + + addCommands(new IntakeSetRollers(0)); + addCommands(new PrintAutonomousTimeElapsed("Path B Blue")); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java index 50a8823..55e94f8 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java +++ b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj.DriverStation; -import org.mayheminc.robot2020.RobotContainer; import frc.robot.Robot; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index c80eb5f..441f072 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -29,7 +29,7 @@ public class Intake extends SubsystemBase implements PidTunerObject { private static final int PIVOT_ZERO_POSITION = 950; public static final double PIVOT_UP = PIVOT_ZERO_POSITION; public static final double PIVOT_SHOOTING = 100.0; - public static final double PIVOT_DOWN = 0.0; + public static final double PIVOT_DOWN = -150.0; private static final double HORIZONTAL_HOLD_OUTPUT = 0.00; private static final double MAX_PID_MOVEMENT_TIME_SEC = 10.0; @@ -63,13 +63,13 @@ void configPivotMotor(MayhemTalonSRX motor) { // If we want 50% power when at the full extreme, // Full extreme is 900 ticks // kP = (0.5 * 1023) / 900 = 0.568 - motor.config_kP(0, 1.2, 0); // based upon Robert's initial calcs, above + motor.config_kP(0, 2.0, 0); // based upon Robert's initial calcs, above // typical value of about 1/100 of kP for starting tuning motor.config_kI(0, 0.0, 0); // typical value of about 10x to 100x of kP for starting tuning - motor.config_kD(0, 150.0, 0); + motor.config_kD(0, 100.0, 0); // motor.config_kD(0, 0.575, 0); // practically always set kF to 0 for position control From ab84b08ee28e41123760093961d815c0dc7d44a2 Mon Sep 17 00:00:00 2001 From: Ken Streeter Date: Sun, 14 Mar 2021 15:39:46 -0400 Subject: [PATCH 111/121] Changes from 11 March 2021 at practice field: Tweaked PathARed.java and added PathBRed.java. --- .../mayheminc/robot2020/RobotContainer.java | 3 +- .../autonomousroutines/PathARed.java | 4 +- .../autonomousroutines/PathBRed.java | 45 +++++++++++++++++++ 3 files changed, 49 insertions(+), 3 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBRed.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 7caddb9..30d5c17 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -100,7 +100,8 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - autonomousPrograms.push(/* 19 */ new DriveTest()); + autonomousPrograms.push(/* 20 */ new DriveTest()); + autonomousPrograms.push(/* 19 */ new PathBRed()); autonomousPrograms.push(/* 18 */ new PathARed()); autonomousPrograms.push(/* 17 */ new PathBBlue()); autonomousPrograms.push(/* 16 */ new PathABlue()); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java index 3e53320..3831c03 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java @@ -33,9 +33,9 @@ public PathARed() { addCommands(new DriveZeroGyro(26.0)); addCommands(new IntakeSetRollers(-1)); - addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + addCommands(new ParallelCommandGroup(new IntakeSetPosition(Intake.PIVOT_DOWN), + new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.5 * 12, 26.0))); - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.5 * 12, 26.0)); addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.8 * 12, -95)); addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 14 * 12, 0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBRed.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBRed.java new file mode 100644 index 0000000..c5f6845 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBRed.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.IntakeSetRollers; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class PathBRed extends SequentialCommandGroup { + /** + * Creates a new PathABlue. + */ + public PathBRed() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands(new DriveZeroGyro(44)); + + addCommands(new IntakeSetRollers(-1)); + + addCommands(new ParallelCommandGroup(new IntakeSetPosition(Intake.PIVOT_DOWN), + new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 9 * 12, 44))); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.5 * 12, -60)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 11 * 12, 0)); + + addCommands(new IntakeSetRollers(0)); + addCommands(new PrintAutonomousTimeElapsed("Path B Red")); + } +} From 762c0ede53a15086d14849c761d37c5eda87bb81 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Thu, 1 Apr 2021 19:27:23 -0400 Subject: [PATCH 112/121] Work on Galactic Search --- src/main/java/frc/robot/Robot.java | 1 + .../mayheminc/robot2020/RobotContainer.java | 35 ++++++++++++++++++- .../autonomousroutines/DriveBarrelRacing.java | 6 ++-- .../autonomousroutines/DriveSlalom.java | 2 +- .../autonomousroutines/PathARed.java | 4 ++- .../robot2020/commands/ShooterCeaseFire.java | 2 +- .../robot2020/subsystems/ShooterWheel.java | 2 +- 7 files changed, 44 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 009c3f4..5238997 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,6 +11,7 @@ import org.mayheminc.robot2020.RobotContainer; +import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 30d5c17..cc3c2db 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -11,9 +11,15 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SelectCommand; import java.util.LinkedList; +import java.util.Map; +import java.util.Map.Entry; +import java.util.function.Supplier; import org.mayheminc.robot2020.autonomousroutines.*; import org.mayheminc.robot2020.commands.*; @@ -52,6 +58,10 @@ public class RobotContainer { public static final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); // private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); + private enum GalacticSearchPath { + PathARed, PathABlue, PathBRed, PathBBlue + } + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -100,7 +110,30 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - autonomousPrograms.push(/* 20 */ new DriveTest()); + SelectCommand driveGalacticSearch = new SelectCommand(() -> { + String path = SmartDashboard.getString("GalacticSearchPath", "unknown"); + Command pathCmd = new PrintCommand("NO PATH FOUND :("); + + switch (path) { + case "path a red": + pathCmd = new PathARed(); + break; + case "path b red": + pathCmd = new PathBRed(); + break; + case "path a blue": + pathCmd = new PathABlue(); + break; + case "path b blue": + pathCmd = new PathBBlue(); + break; + } + + return pathCmd; + }); + + autonomousPrograms.push(/* 21 */ new DriveTest()); + autonomousPrograms.push(/* 20 */ driveGalacticSearch); autonomousPrograms.push(/* 19 */ new PathBRed()); autonomousPrograms.push(/* 18 */ new PathARed()); autonomousPrograms.push(/* 17 */ new PathBBlue()); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java index f26f28c..a625ff0 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java @@ -34,15 +34,15 @@ public DriveBarrelRacing() { addCommands(new TurnToHeading(1.5, 0.6, 180, Direction.RIGHT)); addCommands(new TurnToHeading(1.5, 0.6, -55, Direction.RIGHT)); - addCommands(new DriveStraightOnHeading(0.6, 90, -12)); + addCommands(new DriveStraightOnHeading(0.6, 94, -2)); addCommands(new TurnToHeading(1.5, 0.6, 180, Direction.LEFT)); addCommands(new TurnToHeading(1.5, 0.6, 80, Direction.LEFT)); - addCommands(new DriveStraightOnHeading(0.6, 70, 60)); + addCommands(new DriveStraightOnHeading(0.6, 80, 45)); addCommands(new TurnToHeading(1.5, 0.6, -160, Direction.LEFT)); - addCommands(new DriveStraightOnHeading(0.6, 20 * 12, -178)); + addCommands(new DriveStraightOnHeading(0.6, 20 * 12, -180)); addCommands(new PrintAutonomousTimeElapsed("BarrelRacing")); } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java index fe88b28..e7aeb57 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java @@ -55,7 +55,7 @@ public DriveSlalom() { // COOL TURN ⬆️ addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 30, 135)); addCommands(new TurnToHeading(1, 0.3, -180, TurnToHeading.Direction.RIGHT)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 115, 180)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 180)); addCommands(new TurnToHeading(1, 0.3, -135, TurnToHeading.Direction.RIGHT)); addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 56, -135)); addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 15, -180)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java index 3831c03..65ba7ec 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java @@ -25,6 +25,8 @@ public class PathARed extends SequentialCommandGroup { /** * Creates a new PathABlue. + * + * STARTS IN THE CENTER */ public PathARed() { // Add your commands in the super() call, e.g. @@ -33,7 +35,7 @@ public PathARed() { addCommands(new DriveZeroGyro(26.0)); addCommands(new IntakeSetRollers(-1)); - addCommands(new ParallelCommandGroup(new IntakeSetPosition(Intake.PIVOT_DOWN), + addCommands(new ParallelCommandGroup(/* new IntakeSetPosition(Intake.PIVOT_DOWN), */ new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.5 * 12, 26.0))); addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.8 * 12, -95)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java index 9b03987..1e52e48 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java @@ -33,7 +33,7 @@ public ShooterCeaseFire() { new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new Wait(0.1))); // Lower the hood and intake now that we're done shooting - addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + // addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // ensure intake is lowered,); addCommands(new IntakeSetRollers(0.0)); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java index e95ae74..1e9b26d 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -20,7 +20,7 @@ public class ShooterWheel extends SubsystemBase implements PidTunerObject { private final double SECONDS_PER_MINUTE = 60.0; private final double HUNDRED_MS_PER_SECOND = 10.0; - public static final double IDLE_SPEED = 1500.0; + public static final double IDLE_SPEED = 2200.0; public static final double CLOSE_SHOOTING_SPEED = 3000.0; public static final double INITIATION_LINE_SPEED = 3000.0; public static final double TRENCH_FRONT_SPEED = 3400.0; From d7795457288d5e734ac0e8126d753cd4053182c5 Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Mon, 26 Apr 2021 19:31:31 -0400 Subject: [PATCH 113/121] Finalize at-home autonomous routines --- .../mayheminc/robot2020/RobotContainer.java | 67 ++++++++++--------- .../autonomousroutines/DriveSlalom.java | 4 +- .../autonomousroutines/PathABlue.java | 8 ++- .../autonomousroutines/PathARed.java | 15 +++-- 4 files changed, 54 insertions(+), 40 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index cc3c2db..c86e684 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -34,6 +34,10 @@ * commands, and button mappings) should be declared here. */ public class RobotContainer { + public enum GalacticSearchPath { + PATH_A_RED, PATH_A_BLUE, PATH_B_RED, PATH_B_BLUE, UNKNOWN + } + // The robot's subsystems and commands are defined here... public static final Climber climber = new Climber(); @@ -58,10 +62,6 @@ public class RobotContainer { public static final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); // private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); - private enum GalacticSearchPath { - PathARed, PathABlue, PathBRed, PathBBlue - } - /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -110,29 +110,36 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - SelectCommand driveGalacticSearch = new SelectCommand(() -> { - String path = SmartDashboard.getString("GalacticSearchPath", "unknown"); - Command pathCmd = new PrintCommand("NO PATH FOUND :("); - - switch (path) { - case "path a red": - pathCmd = new PathARed(); - break; - case "path b red": - pathCmd = new PathBRed(); - break; - case "path a blue": - pathCmd = new PathABlue(); - break; - case "path b blue": - pathCmd = new PathBBlue(); - break; - } - - return pathCmd; - }); - - autonomousPrograms.push(/* 21 */ new DriveTest()); + SelectCommand driveGalacticSearch = new SelectCommand( + Map.ofEntries(Map.entry(GalacticSearchPath.PATH_A_RED, new PathARed()), + Map.entry(GalacticSearchPath.PATH_A_BLUE, new PathABlue()), + Map.entry(GalacticSearchPath.PATH_B_RED, new PathBRed()), + Map.entry(GalacticSearchPath.PATH_B_BLUE, new PathBBlue()), + Map.entry(GalacticSearchPath.UNKNOWN, new PrintCommand("-------- no path found"))), + () -> { + String path = SmartDashboard.getString("GalacticSearchPath", "unknown"); + GalacticSearchPath pathValue = GalacticSearchPath.UNKNOWN; + + switch (path) { + case "path a red": + pathValue = GalacticSearchPath.PATH_A_RED; + break; + case "path b red": + pathValue = GalacticSearchPath.PATH_B_RED; + break; + case "path a blue": + pathValue = GalacticSearchPath.PATH_A_BLUE; + break; + case "path b blue": + pathValue = GalacticSearchPath.PATH_B_BLUE; + break; + } + + return pathValue; + }); + + autonomousPrograms.push(/* 22 */ new DriveTest()); + // autonomousPrograms.push(/* 21 */ driveGalacticSearch); autonomousPrograms.push(/* 20 */ driveGalacticSearch); autonomousPrograms.push(/* 19 */ new PathBRed()); autonomousPrograms.push(/* 18 */ new PathARed()); @@ -220,13 +227,13 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new TurretSetRel(-200.0)); DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new TurretSetRel(+200.0)); - DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new TurretSetAbs(+0.0)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new TurretSetAbs(+0.0)); // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new // ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); // Debug Hood - // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new ShooterSetHoodRel(+1000)); - // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new ShooterSetHoodRel(-1000)); + DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetRel(+1000)); + DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetRel(-1000)); // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0)); // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java index e7aeb57..d4ce18a 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java @@ -51,11 +51,11 @@ public DriveSlalom() { // COOL TURN ⬇️ addCommands(new TurnToHeading(1, 0.42, -45, TurnToHeading.Direction.LEFT)); addCommands(new TurnToHeading(1, 0.42, -135, TurnToHeading.Direction.LEFT)); - addCommands(new TurnToHeading(1, 0.42, 120, TurnToHeading.Direction.LEFT)); + addCommands(new TurnToHeading(1, 0.42, 100, TurnToHeading.Direction.LEFT)); // COOL TURN ⬆️ addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 30, 135)); addCommands(new TurnToHeading(1, 0.3, -180, TurnToHeading.Direction.RIGHT)); - addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 180)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 88, 180)); addCommands(new TurnToHeading(1, 0.3, -135, TurnToHeading.Direction.RIGHT)); addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 56, -135)); addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 15, -180)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java index c6a02ed..57f0143 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java @@ -16,6 +16,7 @@ import org.mayheminc.robot2020.subsystems.Intake; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -24,6 +25,8 @@ public class PathABlue extends SequentialCommandGroup { /** * Creates a new PathABlue. + * + * Starts pointing straight from the left of the field */ public PathABlue() { // Add your commands in the super() call, e.g. @@ -36,8 +39,9 @@ public PathABlue() { addCommands(new ParallelCommandGroup(new IntakeSetPosition(Intake.PIVOT_DOWN), new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 10 * 12, 0))); - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 7.5 * 12, -85)); - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 13 * 12, 50)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 9.5 * 12, -85)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 10 * 12, 50)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 3 * 12, 0)); addCommands(new IntakeSetRollers(0)); addCommands(new PrintAutonomousTimeElapsed("Path A Blue")); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java index 65ba7ec..56dc7ba 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java @@ -26,20 +26,23 @@ public class PathARed extends SequentialCommandGroup { /** * Creates a new PathABlue. * - * STARTS IN THE CENTER + * STARTS IN THE CENTER, FACING AHEAD */ public PathARed() { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); - addCommands(new DriveZeroGyro(26.0)); + addCommands(new DriveZeroGyro(0)); addCommands(new IntakeSetRollers(-1)); - addCommands(new ParallelCommandGroup(/* new IntakeSetPosition(Intake.PIVOT_DOWN), */ - new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.5 * 12, 26.0))); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.8 * 12, -95)); - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 14 * 12, 0)); + addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 3 * 12, 0)); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 4 * 12, 40)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 9 * 12, -80)); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 13 * 12, 10)); addCommands(new IntakeSetRollers(0)); addCommands(new PrintAutonomousTimeElapsed("Path A Red")); From dfc2bef509fe93dd55ba2ebf825f2de8cf588f3d Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Mon, 26 Apr 2021 19:32:59 -0400 Subject: [PATCH 114/121] Adjust hood and shooter wheel speed continuously during firing sequence --- .../commands/ShooterFiringSequence.java | 3 +- .../ShooterWheelSetToTargetContinuously.java | 40 +++++++++++++++++++ .../TurretAimToTargetContinuously.java | 2 + 3 files changed, 44 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java index 7143104..d3099ec 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java @@ -38,7 +38,8 @@ public ShooterFiringSequence(double waitDuration) { // revolver turntable, shoot for specified duration. // TODO: should really shoot until no balls detected any more addCommands(new ParallelRaceGroup( // - new TurretAimToTargetContinuously(), // continue aiming while shooting + new ShooterWheelSetToTargetContinuously(), new TurretAimToTargetContinuously(), // continue aiming while + // shooting new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), new Wait(waitDuration))); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java new file mode 100644 index 0000000..9ade87f --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterWheelSetToTargetContinuously extends CommandBase { + double m_rpm; + boolean m_waitForDone; + + /** + * Creates a new ShooterWheelSet + */ + public ShooterWheelSetToTargetContinuously() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_rpm = 3000; // default setting + } + + // Called when the command is initially scheduled. + @Override + public void execute() { + m_rpm = RobotContainer.targeting.getDesiredWheelSpeed(); + RobotContainer.shooterWheel.setSpeed(m_rpm); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java index ad2b463..9ca5346 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java @@ -17,6 +17,7 @@ public class TurretAimToTargetContinuously extends CommandBase { */ public TurretAimToTargetContinuously() { // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); addRequirements(RobotContainer.turret); } @@ -34,6 +35,7 @@ public void execute() { // TODO: if the desired azimuth is "beyond the soft stop" should we turn the // robot? RobotContainer.turret.setPositionAbs(pos); + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); } // Called once the command ends or is interrupted. From adb34868de4e4bd21f222ca16f60ff0c3503e5a5 Mon Sep 17 00:00:00 2001 From: rdeml Date: Fri, 30 Jul 2021 19:11:41 -0400 Subject: [PATCH 115/121] CHeck in just before WPI Battlecry. --- .../org/mayheminc/robot2020/Constants.java | 4 +- .../mayheminc/robot2020/RobotContainer.java | 159 ++++++++++-------- .../autonomousroutines/StartBWShoot3.java | 10 +- .../autonomousroutines/StartBWTrench.java | 16 +- .../autonomousroutines/StartBWTrench5.java | 2 +- .../autonomousroutines/StartFWShoot3.java | 15 +- .../StartFWShoot3ThenToRP.java | 5 +- .../robot2020/commands/ShooterCeaseFire.java | 12 +- .../commands/ShooterCloseFiringSequence.java | 25 ++- .../commands/ShooterFiringSequence.java | 16 +- .../commands/ShooterReadyAimFire.java | 9 +- .../ShooterWheelSetToTargetContinuously.java | 2 +- .../robot2020/subsystems/Chimney.java | 2 +- .../mayheminc/robot2020/subsystems/Hood.java | 20 +-- .../robot2020/subsystems/Intake.java | 8 +- .../robot2020/subsystems/ShooterWheel.java | 8 +- .../robot2020/subsystems/Targeting.java | 6 +- .../robot2020/subsystems/Turret.java | 4 + 18 files changed, 189 insertions(+), 134 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java index 9c9bf17..d4cbcb7 100644 --- a/src/main/java/org/mayheminc/robot2020/Constants.java +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -25,8 +25,8 @@ public final class Talon { public static final int DRIVE_RIGHT_A = 3; // high current public static final int DRIVE_RIGHT_B = 4; // high current - public static final int SHOOTER_WHEEL_LEFT = 5; // high current - public static final int SHOOTER_WHEEL_RIGHT = 17; // high current + public static final int SHOOTER_WHEEL_LEFT = 17; // high current + public static final int SHOOTER_WHEEL_RIGHT = 5; // high current public static final int SHOOTER_FEEDER = 6; public static final int SHOOTER_TURRET = 7; public static final int SHOOTER_HOOD = 8; diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index c86e684..c128914 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -13,8 +13,10 @@ import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SelectCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import java.util.LinkedList; import java.util.Map; @@ -71,10 +73,11 @@ public RobotContainer() { configureAutonomousPrograms(); configureDefaultCommands(); - pidtuner = new PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX, - RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN, - RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN, - RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, intake); + // pidtuner = new + // PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX, + // RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN, + // RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN, + // RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, intake); cameraLights.set(true); } @@ -110,55 +113,56 @@ private void configureDefaultCommands() { private void configureAutonomousPrograms() { LinkedList autonomousPrograms = new LinkedList(); - SelectCommand driveGalacticSearch = new SelectCommand( - Map.ofEntries(Map.entry(GalacticSearchPath.PATH_A_RED, new PathARed()), - Map.entry(GalacticSearchPath.PATH_A_BLUE, new PathABlue()), - Map.entry(GalacticSearchPath.PATH_B_RED, new PathBRed()), - Map.entry(GalacticSearchPath.PATH_B_BLUE, new PathBBlue()), - Map.entry(GalacticSearchPath.UNKNOWN, new PrintCommand("-------- no path found"))), - () -> { - String path = SmartDashboard.getString("GalacticSearchPath", "unknown"); - GalacticSearchPath pathValue = GalacticSearchPath.UNKNOWN; - - switch (path) { - case "path a red": - pathValue = GalacticSearchPath.PATH_A_RED; - break; - case "path b red": - pathValue = GalacticSearchPath.PATH_B_RED; - break; - case "path a blue": - pathValue = GalacticSearchPath.PATH_A_BLUE; - break; - case "path b blue": - pathValue = GalacticSearchPath.PATH_B_BLUE; - break; - } - - return pathValue; - }); - - autonomousPrograms.push(/* 22 */ new DriveTest()); + // SelectCommand driveGalacticSearch = new SelectCommand( + // Map.ofEntries(Map.entry(GalacticSearchPath.PATH_A_RED, new PathARed()), + // Map.entry(GalacticSearchPath.PATH_A_BLUE, new PathABlue()), + // Map.entry(GalacticSearchPath.PATH_B_RED, new PathBRed()), + // Map.entry(GalacticSearchPath.PATH_B_BLUE, new PathBBlue()), + // Map.entry(GalacticSearchPath.UNKNOWN, new PrintCommand("-------- no path + // found"))), + // () -> { + // String path = SmartDashboard.getString("GalacticSearchPath", "unknown"); + // GalacticSearchPath pathValue = GalacticSearchPath.UNKNOWN; + + // switch (path) { + // case "path a red": + // pathValue = GalacticSearchPath.PATH_A_RED; + // break; + // case "path b red": + // pathValue = GalacticSearchPath.PATH_B_RED; + // break; + // case "path a blue": + // pathValue = GalacticSearchPath.PATH_A_BLUE; + // break; + // case "path b blue": + // pathValue = GalacticSearchPath.PATH_B_BLUE; + // break; + // } + + // return pathValue; + // }); + + // autonomousPrograms.push(/* 22 */ new DriveTest()); // autonomousPrograms.push(/* 21 */ driveGalacticSearch); - autonomousPrograms.push(/* 20 */ driveGalacticSearch); - autonomousPrograms.push(/* 19 */ new PathBRed()); - autonomousPrograms.push(/* 18 */ new PathARed()); - autonomousPrograms.push(/* 17 */ new PathBBlue()); - autonomousPrograms.push(/* 16 */ new PathABlue()); - autonomousPrograms.push(/* 15 */ new DriveBouncePath()); - autonomousPrograms.push(/* 14 */ new DriveBarrelRacing()); - autonomousPrograms.push(/* 13 */ new DriveSlalom()); + // autonomousPrograms.push(/* 20 */ driveGalacticSearch); + // autonomousPrograms.push(/* 19 */ new PathBRed()); + // autonomousPrograms.push(/* 18 */ new PathARed()); + // autonomousPrograms.push(/* 17 */ new PathBBlue()); + // autonomousPrograms.push(/* 16 */ new PathABlue()); + // autonomousPrograms.push(/* 15 */ new DriveBouncePath()); + // autonomousPrograms.push(/* 14 */ new DriveBarrelRacing()); + // autonomousPrograms.push(/* 13 */ new DriveSlalom()); autonomousPrograms.push(/* 12 */ new StayStill()); - autonomousPrograms.push(/* 11 */ new StartBWDriveOnlyToRP()); - autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToWall()); - autonomousPrograms.push(/* 09 */ new StartFWDriveOnlyToRP()); - autonomousPrograms.push(/* 08 */ new StartFWDriveOnlyToWall()); - autonomousPrograms.push(/* 07 */ new StartBWShoot3ThenToRP()); - autonomousPrograms.push(/* 06 */ new StartBWShoot3ThenToWall()); + // autonomousPrograms.push(/* 11 */ new StartBWDriveOnlyToRP()); + // autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToWall()); + // autonomousPrograms.push(/* 09 */ new StartFWDriveOnlyToRP()); + // autonomousPrograms.push(/* 08 */ new StartFWDriveOnlyToWall()); + // autonomousPrograms.push(/* 07 */ new StartBWShoot3ThenToRP()); + // autonomousPrograms.push(/* 06 */ new StartBWShoot3ThenToWall()); autonomousPrograms.push(/* 05 */ new StartFWShoot3ThenToRP()); - autonomousPrograms.push(/* 04 */ new StartFWShoot3ThenToWall()); - autonomousPrograms.push(/* 03 */ new StartFWRendezvous()); - autonomousPrograms.push(/* 02 */ new StartBWOppTrench()); + // autonomousPrograms.push(/* 04 */ new StartFWShoot3ThenToWall()); + // autonomousPrograms.push(/* 03 */ new StartFWRendezvous()); + // autonomousPrograms.push(/* 02 */ new StartBWOppTrench()); autonomousPrograms.push(/* 01 */ new StartBWTrench3()); autonomousPrograms.push(/* 00 */ new StartBWTrench5()); @@ -177,7 +181,6 @@ private void configureButtonBindings() { configureDriverPadButtons(); configureOperatorStickButtons(); configureOperatorPadButtons(); - } private void configureDriverStickButtons() { @@ -199,7 +202,6 @@ private void configureDriverStickButtons() { // // zero elements that require zeroing // DRIVER_STICK_BUTTON_EIGHT.whenPressed(new DriveZeroGyro()); // DRIVER_STICK_BUTTON_NINE.whenPressed(new Wait(0)); - } private void configureDriverPadButtons() { @@ -212,10 +214,10 @@ private void configureDriverPadButtons() { // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheelVBus(0.0)); // Debug Turret - // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new - // ShooterSetTurretAbs(-5500));// about -30 degrees - // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new - // ShooterSetTurretAbs(+5500));// about +30 degrees + // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new TurretSetAbs(-5500));// + // about -30 degrees + // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new TurretSetAbs(+5500));// + // about +30 degrees // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));// // about -30 degrees // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new @@ -232,30 +234,39 @@ private void configureDriverPadButtons() { // ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); // Debug Hood - DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetRel(+1000)); - DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetRel(-1000)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetRel(+100)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetRel(-100)); // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0)); // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); // Debug shooter pid velocity - DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ShooterWheelAdjust(50.0)); - DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new ShooterWheelAdjust(-50.0)); - DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0)); - DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenHeld(new ParallelCommandGroup( + // new FeederSet(0.8), + // new ChimneySet(0.8) + // )); + // DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whileHeld(new ShooterWheelAdjust(50.0)); + // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whileHeld(new ShooterWheelAdjust(-50.0)); + // DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whileHeld(new HoodSetRel(50.0)); + // DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whileHeld(new HoodSetRel(-50.0)); + // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0)); + + // DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ChimneySet(-0.7)); + // DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new FeederSet(0.8)); + // DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new + // ShooterWheelSet(ShooterWheel.IDLE_SPEED)); + // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ParallelCommandGroup( + // new ChimneySet(0.0), + // new FeederSet(0.0), + // new ShooterWheelSet(0.0))); DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0)); DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); - // DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new - // ShooterCloseFiringSequence(60.0)); - // DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new - // ShooterCeaseFire()); + DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenHeld(new ShooterCloseFiringSequence(60.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whenPressed(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 240, 0)); DRIVER_PAD.DRIVER_PAD_START_BUTTON.whenPressed(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 240, 0)); - - DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whileHeld(new FeederSet(1.0)); - } private void configureOperatorStickButtons() { @@ -264,8 +275,10 @@ private void configureOperatorStickButtons() { private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new RevolverSetTurntable(0.2)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(Intake.PIVOT_DOWN)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(1.0)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new IntakeSetPosition(Intake.PIVOT_UP)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new TurretSetAbs(0.0)); + // OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new + // IntakeSetPosition(Intake.PIVOT_UP)); // new ShooterSetWheel(1000)); OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySet(1.0)); @@ -283,8 +296,8 @@ private void configureOperatorPadButtons() { // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new TurretSetVBus(-0.4)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new TurretSetVBus(+0.4)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new HoodAdjust(+1000.0)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new HoodAdjust(-1000.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new HoodAdjust(+100.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new HoodAdjust(-100.0)); OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(1.0)); OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-1.0)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java index 184ae4c..be0de1a 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java @@ -26,9 +26,13 @@ public StartBWShoot3() { // first, lower the intake, start the shooter wheel, and wait until the turret // is turned towards the target addCommands(new ParallelCommandGroup( // run the following commands in parallel: - new IntakeSetPosition(Intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.IDLE_SPEED), - new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), - new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + new IntakeSetPosition(Intake.PIVOT_DOWN), // intake down + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), // spin up the shooter + new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), // set the hood to a good + // starting position + new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); // turn + // turret + // around addCommands(new ShooterFiringSequence(1.5)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java index 301527b..8adf283 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java @@ -17,7 +17,7 @@ public class StartBWTrench extends SequentialCommandGroup { /** - * Add your docs here. + * Start backwards, right bumper on the centerline of the goal. */ public StartBWTrench(double extraDistance) { // Note: extra distance planned to be 40 inches @@ -26,7 +26,7 @@ public StartBWTrench(double extraDistance) { addCommands(new StartBWShoot3()); // then, drive to the trench entrance (jog left a little to get there) - addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 30, 140)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 35, 125)); // pick up balls while heading down the trench. addCommands(new ParallelCommandGroup( @@ -45,14 +45,16 @@ public StartBWTrench(double extraDistance) { // after getting all three balls, go back to shooting position // first, make sure we drive straight out from under the control panel addCommands(new DriveStraightOnHeading(-0.6, DistanceUnits.INCHES, 16 + extraDistance, 180)); - addCommands(new IntakeSetRollers(0.0)); // turn off the intake in case it has been stalled for a while + addCommands(new IntakeSetRollers(-0.2)); // turn off the intake in case it has been stalled for a while // drive diagonally over towards the shooting position, while turning on shooter // wheels, raising the hood, and re-aiming the turret - addCommands(new ParallelCommandGroup(new ShooterWheelSet(ShooterWheel.IDLE_SPEED), - new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), - new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)), - new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160))); + addCommands( // + new ParallelCommandGroup( // + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), // + new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), // + new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)), // + new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160))); // // straighten out again to enable turret to aim to the target addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 24, 180)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java index 9939116..204da84 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java @@ -14,6 +14,6 @@ public class StartBWTrench5 extends SequentialCommandGroup { * Add your docs here. */ public StartBWTrench5() { - addCommands(new StartBWTrench(40)); // use extraDistance of 40 inches for control panel + addCommands(new StartBWTrench(50)); // use extraDistance of 40 inches for control panel } } diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java index 1d5d0d4..6ca58b8 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java @@ -25,13 +25,14 @@ public StartFWShoot3() { // first, lower the intake, start the shooter wheel, and wait until the turret // is turned towards the target - addCommands(new ParallelCommandGroup( // run the following commands in parallel: - new IntakeSetPosition(Intake.PIVOT_DOWN), - new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED), - new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), - new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); - - addCommands(new ShooterReadyAimFire(1.0)); + // addCommands(new ParallelCommandGroup( // run the following commands in + // parallel: + // new IntakeSetPosition(Intake.PIVOT_DOWN), + // // new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED, true), + // // new HoodSetAbs(Hood.INITIATION_LINE_POSITION), + // new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + addCommands(new ShooterFiringSequence(4.0)); // turn the shooter wheel and intake off now that the shooting is all done addCommands(new ParallelCommandGroup( // below commands in parallel diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java index ac8b4dc..d81358b 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java @@ -18,7 +18,10 @@ public class StartFWShoot3ThenToRP extends SequentialCommandGroup { public StartFWShoot3ThenToRP() { // start backwards and shoot the first three balls - addCommands(new StartFWShoot3()); + // addCommands(new StartFWShoot3()); + addCommands(new Wait(2.0)); + + addCommands(new ShooterFiringSequence(4.0)); // then, drive to the rendezvous point addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 0)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java index 1e52e48..f4a93f4 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java @@ -29,11 +29,17 @@ public ShooterCeaseFire() { // turn off the subsystems that were used in shooting // (feed roller, chimney, revolver, and shooter wheels) - addCommands(new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0), - new ShooterWheelSet(ShooterWheel.IDLE_SPEED), new Wait(0.1))); + addCommands( // + new ParallelRaceGroup( // + new FeederSet(0.0), // + new ChimneySet(0.0), // + new RevolverSetTurntable(0.0), // + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), // + new Wait(0.1) // + )); // Lower the hood and intake now that we're done shooting - // addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // ensure intake is lowered,); addCommands(new IntakeSetRollers(0.0)); } diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java index 62d619c..b6d6a0d 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java @@ -29,21 +29,32 @@ public ShooterCloseFiringSequence(double waitDuration) { // Prepare for shooting. addCommands(new IntakeSetPosition(Intake.PIVOT_SHOOTING)); // move intake to "shooting position" - addCommands(new ParallelCommandGroup( // prepare for shooting, + + addCommands( // + // new ParallelCommandGroup( // prepare for shooting, new AirCompressorPause() // turn off compressor while actively shooting, // , new ShooterAimToTarget() // and aim at the target (azimuth and elevation). - )); + // ) + ); // no aiming when up close; just turn on the shooter wheels and raise the hood - addCommands(new ShooterWheelSet(ShooterWheel.CLOSE_SHOOTING_SPEED), // shooter wheel manual speed - new HoodSetAbs(Hood.CLOSE_SHOOTING_POSITION)); + addCommands( // + new ParallelCommandGroup( // wait until shooter and hood are good. + new ShooterWheelSet(ShooterWheel.CLOSE_SHOOTING_SPEED, true), // + new HoodSetAbs(Hood.CLOSE_SHOOTING_POSITION) // + ) // + ); // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the // revolver turntable, shoot for specified duration. // TODO: should really shoot until no balls detected any more - addCommands(new ParallelRaceGroup( // - new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), - new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), new Wait(waitDuration))); + addCommands( // + new ParallelRaceGroup( // + new FeederSet(1.0), // + new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), // + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), // + new Wait(waitDuration)) // + ); // turn off the feeder, chimney, and revolver, ending after 0.1 seconds addCommands(new ShooterCeaseFire()); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java index d3099ec..59d61f0 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java @@ -32,16 +32,22 @@ public ShooterFiringSequence(double waitDuration) { new ShooterAimToTarget())); // and aim at the target (azimuth and elevation). // prior command established aim; turn on the shooter wheels and maintain turret - addCommands(new ParallelRaceGroup(new ShooterWheelSetToTarget(true), new TurretAimToTargetContinuously())); + addCommands( // + new ParallelRaceGroup( // + new ShooterWheelSetToTarget(true), // set the shooter wheel + new TurretAimToTargetContinuously() // set the aim hood and turret to target + )); // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the // revolver turntable, shoot for specified duration. // TODO: should really shoot until no balls detected any more addCommands(new ParallelRaceGroup( // - new ShooterWheelSetToTargetContinuously(), new TurretAimToTargetContinuously(), // continue aiming while - // shooting - new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), - new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), new Wait(waitDuration))); + new ShooterWheelSetToTargetContinuously(), // spin up the shooter wheel + new TurretAimToTargetContinuously(), // continue aiming while shooting + new FeederSet(1.0), // turn on the shooter feeder + new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), // wait a bit and then set the chimney + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), // + new Wait(waitDuration))); // wait a little longer and then turn on the revolver // turn off the feeder, chimney, and revolver, ending after 0.1 seconds addCommands(new ShooterCeaseFire()); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java index ee279f9..ea1ac36 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -37,9 +37,12 @@ public ShooterReadyAimFire(double waitDuration) { // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the // revolver turntable, shoot for specified duration // TODO: should really shoot until no balls detected any more - addCommands( - new ParallelRaceGroup(new FeederSet(1.0), new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), - new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), new Wait(waitDuration))); + addCommands(new ParallelRaceGroup( // + new FeederSet(1.0), // + new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), // + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), // + new Wait(waitDuration) // + )); // turn off the feeder, chimney, and revolver, ending after 0.1 seconds addCommands( diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java index 9ade87f..bc85a41 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java @@ -13,7 +13,7 @@ public class ShooterWheelSetToTargetContinuously extends CommandBase { double m_rpm; - boolean m_waitForDone; + // boolean m_waitForDone; /** * Creates a new ShooterWheelSet diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java index 9916316..6317f76 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -34,7 +34,7 @@ public Chimney() { chimneyTalon.configPeakOutputForward(+12.0); chimneyTalon.configPeakOutputReverse(-12.0); - chimneyTalon.setInverted(true); + chimneyTalon.setInverted(false); } @Override diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java index 04bd78c..c63c085 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java @@ -15,14 +15,14 @@ public class Hood extends SubsystemBase implements PidTunerObject { private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); private final static int MIN_POSITION = 0; - private final static int MAX_POSITION = 90000; - private final static double POSITION_TOLERANCE = 1000.0; + private final static int MAX_POSITION = 8500; + private final static double POSITION_TOLERANCE = 20.0; public final static double STARTING_POSITION = 0; - public final static double TARGET_ZONE_POSITION = 5000; - public final static double CLOSE_SHOOTING_POSITION = 30000; - public final static double INITIATION_LINE_POSITION = 65000; - public final static double TRENCH_MID_POSITION = 80000; + public final static double TARGET_ZONE_POSITION = 500; + public final static double CLOSE_SHOOTING_POSITION = 100; + public final static double INITIATION_LINE_POSITION = 4500; + public final static double TRENCH_MID_POSITION = 8000; private double m_desiredPosition = 0.0; @@ -38,7 +38,7 @@ public void init() { } private void configureTalon() { - hoodTalon.config_kP(0, 1.0, 0); + hoodTalon.config_kP(0, 10.0, 0); hoodTalon.config_kI(0, 0.0, 0); hoodTalon.config_kD(0, 0.0, 0); hoodTalon.config_kF(0, 0.0, 0); @@ -48,9 +48,9 @@ private void configureTalon() { hoodTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); hoodTalon.configNominalOutputVoltage(+0.0f, -0.0f); - hoodTalon.configPeakOutputVoltage(+12.0, -12.0); - hoodTalon.setInverted(true); - hoodTalon.setSensorPhase(true); + hoodTalon.configPeakOutputVoltage(+6.0, -6.0); + hoodTalon.setInverted(false); + hoodTalon.setSensorPhase(false); hoodTalon.configForwardSoftLimitThreshold(MAX_POSITION); hoodTalon.configForwardSoftLimitEnable(true); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 441f072..9fef8e9 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -29,7 +29,7 @@ public class Intake extends SubsystemBase implements PidTunerObject { private static final int PIVOT_ZERO_POSITION = 950; public static final double PIVOT_UP = PIVOT_ZERO_POSITION; public static final double PIVOT_SHOOTING = 100.0; - public static final double PIVOT_DOWN = -150.0; + public static final double PIVOT_DOWN = -350.0; private static final double HORIZONTAL_HOLD_OUTPUT = 0.00; private static final double MAX_PID_MOVEMENT_TIME_SEC = 10.0; @@ -63,13 +63,13 @@ void configPivotMotor(MayhemTalonSRX motor) { // If we want 50% power when at the full extreme, // Full extreme is 900 ticks // kP = (0.5 * 1023) / 900 = 0.568 - motor.config_kP(0, 2.0, 0); // based upon Robert's initial calcs, above + motor.config_kP(0, 1.5, 0); // based upon Robert's initial calcs, above // typical value of about 1/100 of kP for starting tuning motor.config_kI(0, 0.0, 0); // typical value of about 10x to 100x of kP for starting tuning - motor.config_kD(0, 100.0, 0); + motor.config_kD(0, 50.0, 0); // motor.config_kD(0, 0.575, 0); // practically always set kF to 0 for position control @@ -140,7 +140,7 @@ private void updatePivotPower() { if (pivotTalon.getPosition() > (PIVOT_UP / 2.0)) { setPivotVBus(+0.05); } else { // we are close to the PIVOT DOWN, so apply a little negative power. - setPivotVBus(-0.05); + setPivotVBus(-0.05 * 2); } } else { pivotTalon.set(ControlMode.Position, m_targetPosition, DemandType.ArbitraryFeedForward, m_feedForward); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java index 1e9b26d..389436b 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -20,11 +20,11 @@ public class ShooterWheel extends SubsystemBase implements PidTunerObject { private final double SECONDS_PER_MINUTE = 60.0; private final double HUNDRED_MS_PER_SECOND = 10.0; - public static final double IDLE_SPEED = 2200.0; - public static final double CLOSE_SHOOTING_SPEED = 3000.0; - public static final double INITIATION_LINE_SPEED = 3000.0; + public static final double IDLE_SPEED = 1000.0; + public static final double CLOSE_SHOOTING_SPEED = 4000.0; + public static final double INITIATION_LINE_SPEED = 4500.0; public static final double TRENCH_FRONT_SPEED = 3400.0; - public static final double MAX_SPEED_RPM = 3600; + public static final double MAX_SPEED_RPM = 5000; double m_targetSpeedRPM; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java index 4bb7053..93e6a9e 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -238,7 +238,8 @@ private double getHoodFromY() { // too close for the lob shot, switch to the bullet shot return CLOSE_HOOD_ANGLE; } else { - return 105874 + -407324 * m_bestY + 881911 * m_bestY * m_bestY + -506286 * m_bestY * m_bestY * m_bestY; + // y(x) = -33,064x^2 + 37,144x^1 + -5,872 + return -33064 * m_bestY * m_bestY + 37144*m_bestY - 5872; } } @@ -248,7 +249,8 @@ private double getHoodFromY() { * @return */ private double getWheelSpeedFromY() { - double computedWheelSpeed = 2618 + -1939 * m_bestY + 3583 * m_bestY * m_bestY; + double computedWheelSpeed = -137 + 14201 * m_bestY + -10089 * m_bestY * m_bestY; + // -10,089x^2 + 14,201x^1 + -137 if (m_bestY < BEST_Y_CLOSE_THRESHOLD) { // too close for the lob shot, switch to the bullet shot diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java index 5a36fda..912211b 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -74,6 +74,10 @@ void configureTurretTalon() { turretTalon.configReverseSoftLimitThreshold(MIN_POSITION); turretTalon.configReverseSoftLimitEnable(true); + + turretTalon.setInverted(false); + turretTalon.setSensorPhase(false); + this.setVBus(0.0); } From 796abe0f26c91001f66b6a4ca4ab87b899dbdaca Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Tue, 24 Aug 2021 20:12:35 -0400 Subject: [PATCH 116/121] Impose current limit on drive motors --- .../org/mayheminc/robot2020/subsystems/Drive.java | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 0bb1d0f..23759bf 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.*; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; //import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -101,7 +102,7 @@ public Drive() { m_HeadingPid = new PIDController(HEADING_PID_P, HEADING_PID_I, HEADING_PID_D, m_HeadingError, m_HeadingCorrection, 0.020 /* period in seconds */); m_HeadingPid.setInputRange(-180.0f, 180.0f); - m_HeadingPid.setContinuous(true); // treats the input range as "continous" with wrap-around + m_HeadingPid.setContinuous(true); // treats the input range as "continuous" with wrap-around m_HeadingPid.setOutputRange(-.50, .50); // set the maximum power to correct twist m_HeadingPid.setAbsoluteTolerance(kToleranceDegreesPIDControl); @@ -111,6 +112,11 @@ public Drive() { rightFrontTalon.setNeutralMode(NeutralMode.Coast); rightRearTalon.setNeutralMode(NeutralMode.Coast); + this.configTalon(leftFrontTalon); + this.configTalon(leftRearTalon); + this.configTalon(rightFrontTalon); + this.configTalon(rightRearTalon); + // configure output voltages leftFrontTalon.configNominalOutputVoltage(+0.0f, -0.0f); leftRearTalon.configNominalOutputVoltage(+0.0f, -0.0f); @@ -147,6 +153,12 @@ public Drive() { configureDriveTalon(rightFrontTalon); } + private void configTalon(TalonSRX talon) { + talon.configPeakCurrentLimit(60); + talon.configContinuousCurrentLimit(40); + talon.configPeakCurrentDuration(3000); + } + public void init() { // reset the NavX zeroHeadingGyro(0.0); From f4292df4d45e88a4cca9e7bdbf9210783a522e9a Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Tue, 24 Aug 2021 20:26:21 -0400 Subject: [PATCH 117/121] Add currentLimit to MayhemTalonSRX --- .../mayheminc/robot2020/subsystems/Climber.java | 7 +++++-- .../org/mayheminc/robot2020/subsystems/Drive.java | 13 +++++++++---- .../org/mayheminc/robot2020/subsystems/Hood.java | 3 ++- .../org/mayheminc/robot2020/subsystems/Intake.java | 3 ++- .../mayheminc/robot2020/subsystems/Revolver.java | 5 +++-- .../robot2020/subsystems/ShooterWheel.java | 7 +++++-- .../org/mayheminc/robot2020/subsystems/Turret.java | 7 ++++--- .../java/org/mayheminc/util/MayhemTalonSRX.java | 14 +++++++++++++- 8 files changed, 43 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java index 5f3f939..d4c02a8 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -12,6 +12,7 @@ import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -21,8 +22,10 @@ public class Climber extends SubsystemBase { private static final int MAX_HEIGHT_SOFT_LIMIT = 640000; private static final int MIN_HEIGHT_SOFT_LIMIT = 5000; - private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT); - private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT); + private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT, + CurrentLimit.LOW_CURRENT); + private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT, + CurrentLimit.LOW_CURRENT); // private final MayhemTalonSRX walkerLeft = new // MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); // private final MayhemTalonSRX walkerRight = new diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java index 23759bf..cf926cb 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -19,6 +19,7 @@ import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.PidTunerObject; import org.mayheminc.util.Utils; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; // TODO: Address all deprecated code masked by @SuppressWarnings("removal") annotations (not just in Drive.java) @SuppressWarnings("removal") @@ -38,10 +39,14 @@ public class Drive extends SubsystemBase implements PidTunerObject { private static final int LOOPS_GYRO_DELAY = 10; // Talons - private final MayhemTalonSRX leftFrontTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_LEFT_A); - private final MayhemTalonSRX leftRearTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_LEFT_B); - private final MayhemTalonSRX rightFrontTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_RIGHT_A); - private final MayhemTalonSRX rightRearTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_RIGHT_B); + private final MayhemTalonSRX leftFrontTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_LEFT_A, + CurrentLimit.HIGH_CURRENT); + private final MayhemTalonSRX leftRearTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_LEFT_B, + CurrentLimit.HIGH_CURRENT); + private final MayhemTalonSRX rightFrontTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_RIGHT_A, + CurrentLimit.HIGH_CURRENT); + private final MayhemTalonSRX rightRearTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_RIGHT_B, + CurrentLimit.HIGH_CURRENT); // Sensors private AHRS Navx; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java index c63c085..82a332d 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java @@ -10,9 +10,10 @@ import org.mayheminc.util.MayhemTalonSRX; // import org.mayheminc.util.PidTuner; import org.mayheminc.util.PidTunerObject; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; public class Hood extends SubsystemBase implements PidTunerObject { - private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD); + private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD, CurrentLimit.LOW_CURRENT); private final static int MIN_POSITION = 0; private final static int MAX_POSITION = 8500; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index 9fef8e9..e77fb9c 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -16,6 +16,7 @@ import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.PidTunerObject; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -25,7 +26,7 @@ public class Intake extends SubsystemBase implements PidTunerObject { private final int PIVOT_CLOSE_ENOUGH = 50; private final VictorSPX rollersTalon = new VictorSPX(Constants.Talon.INTAKE_ROLLERS); - private final MayhemTalonSRX pivotTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_PIVOT); + private final MayhemTalonSRX pivotTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_PIVOT, CurrentLimit.LOW_CURRENT); private static final int PIVOT_ZERO_POSITION = 950; public static final double PIVOT_UP = PIVOT_ZERO_POSITION; public static final double PIVOT_SHOOTING = 100.0; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java b/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java index a19fceb..83121bd 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java @@ -16,9 +16,11 @@ import org.mayheminc.robot2020.Constants; import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; public class Revolver extends SubsystemBase { - private final MayhemTalonSRX revolverTalon = new MayhemTalonSRX(Constants.Talon.REVOLVER_TURNTABLE); + private final MayhemTalonSRX revolverTalon = new MayhemTalonSRX(Constants.Talon.REVOLVER_TURNTABLE, + CurrentLimit.LOW_CURRENT); /** * Creates a new Revolver. @@ -44,7 +46,6 @@ void updateSmartDashboard() { SmartDashboard.putNumber("Revolver Speed", revolverTalon.getSpeed()); } - public void setRevolverSpeed(double speed) { revolverTalon.set(ControlMode.PercentOutput, speed); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java index 389436b..bebc476 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -10,10 +10,13 @@ import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.PidTunerObject; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; public class ShooterWheel extends SubsystemBase implements PidTunerObject { - private final MayhemTalonSRX shooterWheelLeft = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_LEFT); - private final MayhemTalonSRX shooterWheelRight = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_RIGHT); + private final MayhemTalonSRX shooterWheelLeft = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_LEFT, + CurrentLimit.HIGH_CURRENT); + private final MayhemTalonSRX shooterWheelRight = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_RIGHT, + CurrentLimit.HIGH_CURRENT); // private final double MAX_SPEED_RPM = 5760.0; private final double TALON_TICKS_PER_REV = 2048.0; diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java index 912211b..8871ac4 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -12,9 +12,11 @@ import org.mayheminc.util.History; import org.mayheminc.util.MayhemTalonSRX; import org.mayheminc.util.PidTunerObject; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; public class Turret extends SubsystemBase implements PidTunerObject { - private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET); + private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET, + CurrentLimit.LOW_CURRENT); public static final boolean WAIT_FOR_DONE = true; private final int MIN_POSITION = -12500; // approx 90 degrees @@ -74,10 +76,9 @@ void configureTurretTalon() { turretTalon.configReverseSoftLimitThreshold(MIN_POSITION); turretTalon.configReverseSoftLimitEnable(true); - turretTalon.setInverted(false); turretTalon.setSensorPhase(false); - + this.setVBus(0.0); } diff --git a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java index f834a0b..dab0601 100644 --- a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java +++ b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -8,13 +8,17 @@ public class MayhemTalonSRX extends TalonSRX { + public enum CurrentLimit { + HIGH_CURRENT, LOW_CURRENT + } + ControlMode controlMode; double p; double i; double d; double f; - public MayhemTalonSRX(int deviceNumber) { + public MayhemTalonSRX(int deviceNumber, CurrentLimit currentLimit) { super(deviceNumber); this.configFactoryDefault(); @@ -28,6 +32,14 @@ public MayhemTalonSRX(int deviceNumber) { this.setNeutralMode(NeutralMode.Coast); + if (currentLimit == CurrentLimit.HIGH_CURRENT) { + this.configPeakCurrentLimit(60); + this.configContinuousCurrentLimit(40); + this.configPeakCurrentDuration(3000); + } else if (currentLimit == CurrentLimit.LOW_CURRENT) { + this.configContinuousCurrentLimit(30); + } + // this.configContinuousCurrentLimit(0, 0); // this.configPeakCurrentLimit(0, 0); // this.configPeakCurrentDuration(0, 0); From 5498c7173073deb482f2657f94fda435f1b457dc Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Tue, 24 Aug 2021 20:45:13 -0400 Subject: [PATCH 118/121] Change current duration to 1s --- src/main/java/org/mayheminc/util/MayhemTalonSRX.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java index dab0601..8e6596d 100644 --- a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java +++ b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -35,7 +35,7 @@ public MayhemTalonSRX(int deviceNumber, CurrentLimit currentLimit) { if (currentLimit == CurrentLimit.HIGH_CURRENT) { this.configPeakCurrentLimit(60); this.configContinuousCurrentLimit(40); - this.configPeakCurrentDuration(3000); + this.configPeakCurrentDuration(1000); } else if (currentLimit == CurrentLimit.LOW_CURRENT) { this.configContinuousCurrentLimit(30); } From 79e79ae5540100146c7e4abcb7793ecb486733e1 Mon Sep 17 00:00:00 2001 From: robertdeml Date: Tue, 26 Oct 2021 19:28:16 -0400 Subject: [PATCH 119/121] Various motor things --- .../mayheminc/robot2020/RobotContainer.java | 8 +- .../commands/ShooterCloseFiringSequence.java | 73 +++++++++---------- .../mayheminc/robot2020/subsystems/Hood.java | 12 +-- .../robot2020/subsystems/Intake.java | 4 +- .../robot2020/subsystems/Turret.java | 4 +- 5 files changed, 47 insertions(+), 54 deletions(-) diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index c128914..49dec7e 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -287,8 +287,8 @@ private void configureOperatorPadButtons() { OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whenPressed(new ShooterAimToTarget()); OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollersWhileHeld(1.0)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(true)); - OPERATOR_PAD.OPERATOR_PAD_BUTTON_TEN.whenPressed(new ClimberSetPistons(false)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(false)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TEN.whenPressed(new ClimberSetPistons(true)); // OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new // IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); @@ -296,8 +296,8 @@ private void configureOperatorPadButtons() { // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new TurretSetVBus(-0.4)); OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new TurretSetVBus(+0.4)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new HoodAdjust(+100.0)); - OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new HoodAdjust(-100.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new HoodAdjust(+1000.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new HoodAdjust(-1000.0)); OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(1.0)); OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-1.0)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java index b6d6a0d..5f5b9a9 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java @@ -19,44 +19,37 @@ // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html public class ShooterCloseFiringSequence extends SequentialCommandGroup { - /** - * Creates a new ShooterReadyAimFire. - */ - public ShooterCloseFiringSequence(double waitDuration) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - super(); - - // Prepare for shooting. - addCommands(new IntakeSetPosition(Intake.PIVOT_SHOOTING)); // move intake to "shooting position" - - addCommands( // - // new ParallelCommandGroup( // prepare for shooting, - new AirCompressorPause() // turn off compressor while actively shooting, - // , new ShooterAimToTarget() // and aim at the target (azimuth and elevation). - // ) - ); - - // no aiming when up close; just turn on the shooter wheels and raise the hood - addCommands( // - new ParallelCommandGroup( // wait until shooter and hood are good. - new ShooterWheelSet(ShooterWheel.CLOSE_SHOOTING_SPEED, true), // - new HoodSetAbs(Hood.CLOSE_SHOOTING_POSITION) // - ) // - ); - - // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the - // revolver turntable, shoot for specified duration. - // TODO: should really shoot until no balls detected any more - addCommands( // - new ParallelRaceGroup( // - new FeederSet(1.0), // - new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), // - new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), // - new Wait(waitDuration)) // - ); - - // turn off the feeder, chimney, and revolver, ending after 0.1 seconds - addCommands(new ShooterCeaseFire()); - } + /** + * Creates a new ShooterReadyAimFire. + */ + public ShooterCloseFiringSequence(double waitDuration) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + // Prepare for shooting. + addCommands(new IntakeSetPosition(Intake.PIVOT_SHOOTING)); // move intake to "shooting position" + + addCommands( // + // new ParallelCommandGroup( // prepare for shooting, + new AirCompressorPause() // turn off compressor while actively shooting, + // , new ShooterAimToTarget() // and aim at the target (azimuth and elevation). + // ) + ); + + // no aiming when up close; just turn on the shooter wheels and raise the hood + addCommands(new ShooterWheelSet(ShooterWheel.CLOSE_SHOOTING_SPEED, true), // shooter wheel manual speed + new HoodSetAbs(Hood.CLOSE_SHOOTING_POSITION)); + + // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the + // revolver turntable, shoot for specified duration. + // TODO: should really shoot until no balls detected any more + addCommands(new ParallelRaceGroup( // + new FeederSet(1.0), // + new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), // + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), new Wait(waitDuration))); + + // turn off the feeder, chimney, and revolver, ending after 0.1 seconds + addCommands(new ShooterCeaseFire()); + } } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java index 82a332d..881fcff 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java @@ -16,13 +16,13 @@ public class Hood extends SubsystemBase implements PidTunerObject { private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD, CurrentLimit.LOW_CURRENT); private final static int MIN_POSITION = 0; - private final static int MAX_POSITION = 8500; - private final static double POSITION_TOLERANCE = 20.0; + private final static int MAX_POSITION = 8000; + private final static double POSITION_TOLERANCE = 100.0; public final static double STARTING_POSITION = 0; - public final static double TARGET_ZONE_POSITION = 500; - public final static double CLOSE_SHOOTING_POSITION = 100; - public final static double INITIATION_LINE_POSITION = 4500; + public final static double TARGET_ZONE_POSITION = 5000; + public final static double CLOSE_SHOOTING_POSITION = 0; + public final static double INITIATION_LINE_POSITION = 5000; public final static double TRENCH_MID_POSITION = 8000; private double m_desiredPosition = 0.0; @@ -49,7 +49,7 @@ private void configureTalon() { hoodTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); hoodTalon.configNominalOutputVoltage(+0.0f, -0.0f); - hoodTalon.configPeakOutputVoltage(+6.0, -6.0); + hoodTalon.configPeakOutputVoltage(+12.0, -12.0); hoodTalon.setInverted(false); hoodTalon.setSensorPhase(false); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index e77fb9c..da7b450 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -30,7 +30,7 @@ public class Intake extends SubsystemBase implements PidTunerObject { private static final int PIVOT_ZERO_POSITION = 950; public static final double PIVOT_UP = PIVOT_ZERO_POSITION; public static final double PIVOT_SHOOTING = 100.0; - public static final double PIVOT_DOWN = -350.0; + public static final double PIVOT_DOWN = -240.0; private static final double HORIZONTAL_HOLD_OUTPUT = 0.00; private static final double MAX_PID_MOVEMENT_TIME_SEC = 10.0; @@ -141,7 +141,7 @@ private void updatePivotPower() { if (pivotTalon.getPosition() > (PIVOT_UP / 2.0)) { setPivotVBus(+0.05); } else { // we are close to the PIVOT DOWN, so apply a little negative power. - setPivotVBus(-0.05 * 2); + setPivotVBus(-0.10); } } else { pivotTalon.set(ControlMode.Position, m_targetPosition, DemandType.ArbitraryFeedForward, m_feedForward); diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java index 8871ac4..0f5f3ec 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -19,8 +19,8 @@ public class Turret extends SubsystemBase implements PidTunerObject { CurrentLimit.LOW_CURRENT); public static final boolean WAIT_FOR_DONE = true; - private final int MIN_POSITION = -12500; // approx 90 degrees - private final int MAX_POSITION = +26000; // approx 180 degrees + private final int MIN_POSITION = -38000; // approx -270 degrees + private final int MAX_POSITION = +3000; // approx +25 degrees private final int DESTINATION_TOLERANCE = 200; // if "at destination" want the "I" to get us as close as possible From 787b123ecbd4f52696f4c28541553f3252868aaf Mon Sep 17 00:00:00 2001 From: Caleb Denio Date: Thu, 28 Oct 2021 22:28:28 -0400 Subject: [PATCH 120/121] Some changes for RiverRage --- .../mayheminc/robot2020/RobotContainer.java | 10 +++--- .../RiverRageForwardShoot3.java | 31 +++++++++++++++++++ .../robot2020/subsystems/ShooterWheel.java | 4 +-- 3 files changed, 39 insertions(+), 6 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/autonomousroutines/RiverRageForwardShoot3.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index 49dec7e..bed3082 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -152,19 +152,20 @@ private void configureAutonomousPrograms() { // autonomousPrograms.push(/* 15 */ new DriveBouncePath()); // autonomousPrograms.push(/* 14 */ new DriveBarrelRacing()); // autonomousPrograms.push(/* 13 */ new DriveSlalom()); - autonomousPrograms.push(/* 12 */ new StayStill()); + autonomousPrograms.push(new RiverRageForwardShoot3()); + // autonomousPrograms.push(/* 12 */ new StayStill()); // autonomousPrograms.push(/* 11 */ new StartBWDriveOnlyToRP()); // autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToWall()); // autonomousPrograms.push(/* 09 */ new StartFWDriveOnlyToRP()); // autonomousPrograms.push(/* 08 */ new StartFWDriveOnlyToWall()); // autonomousPrograms.push(/* 07 */ new StartBWShoot3ThenToRP()); // autonomousPrograms.push(/* 06 */ new StartBWShoot3ThenToWall()); - autonomousPrograms.push(/* 05 */ new StartFWShoot3ThenToRP()); + // autonomousPrograms.push(/* 05 */ new StartFWShoot3ThenToRP()); // autonomousPrograms.push(/* 04 */ new StartFWShoot3ThenToWall()); // autonomousPrograms.push(/* 03 */ new StartFWRendezvous()); // autonomousPrograms.push(/* 02 */ new StartBWOppTrench()); - autonomousPrograms.push(/* 01 */ new StartBWTrench3()); - autonomousPrograms.push(/* 00 */ new StartBWTrench5()); + // autonomousPrograms.push(/* 01 */ new StartBWTrench3()); + // autonomousPrograms.push(/* 00 */ new StartBWTrench5()); autonomous.setAutonomousPrograms(autonomousPrograms); @@ -229,6 +230,7 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new TurretSetRel(-200.0)); DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new TurretSetRel(+200.0)); + DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new DriveStraightOnHeading(-0.25, 8, 0)); // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new TurretSetAbs(+0.0)); // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new // ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/RiverRageForwardShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/RiverRageForwardShoot3.java new file mode 100644 index 0000000..c645c74 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/RiverRageForwardShoot3.java @@ -0,0 +1,31 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.ShooterCloseFiringSequence; +import org.mayheminc.robot2020.commands.ShooterWheelSet; +import org.mayheminc.robot2020.subsystems.ShooterWheel; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class RiverRageForwardShoot3 extends SequentialCommandGroup { + /** Creates a new RiverRageForwardShoot3. */ + public RiverRageForwardShoot3() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new DriveZeroGyro(0)); + + addCommands(new DriveStraightOnHeading(0.4, 6 * 14, 0)); + + addCommands(new ShooterWheelSet(ShooterWheel.CLOSE_SHOOTING_SPEED, true)); + + addCommands(new ShooterCloseFiringSequence(15)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java index bebc476..15e2fd2 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -60,8 +60,8 @@ private void configureWheelFalcons() { configureOneWheelFalcon(shooterWheelRight); // with the exception of one rotating the opposite direction - shooterWheelLeft.setInverted(false); - shooterWheelRight.setInverted(true); + shooterWheelLeft.setInverted(true); + shooterWheelRight.setInverted(false); } private void configureOneWheelFalcon(MayhemTalonSRX shooterWheelFalcon) { From 519ce31b08ded7ce5174e887eaee164f800c0d26 Mon Sep 17 00:00:00 2001 From: Robert Deml Date: Thu, 4 Nov 2021 21:14:02 -0400 Subject: [PATCH 121/121] added ChimneyUnjam.java Trying to fix the drive straight backwards. The intake motor is broke, so the intake is now gravity powered. --- .../mayheminc/robot2020/RobotContainer.java | 4 ++-- .../autonomousroutines/DriveSlalom.java | 4 ++-- .../robot2020/commands/ChimneyUnjam.java | 18 ++++++++++++++++++ .../commands/DriveStraightOnHeading.java | 4 ++++ .../mayheminc/robot2020/subsystems/Intake.java | 16 ++++++++++------ 5 files changed, 36 insertions(+), 10 deletions(-) create mode 100644 src/main/java/org/mayheminc/robot2020/commands/ChimneyUnjam.java diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java index bed3082..0e87513 100644 --- a/src/main/java/org/mayheminc/robot2020/RobotContainer.java +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -230,7 +230,7 @@ private void configureDriverPadButtons() { DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new TurretSetRel(-200.0)); DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new TurretSetRel(+200.0)); - DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new DriveStraightOnHeading(-0.25, 8, 0)); + DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new DriveStraightOnHeading(-0.25, 8)); // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new TurretSetAbs(+0.0)); // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new // ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); @@ -306,7 +306,7 @@ private void configureOperatorPadButtons() { // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new // RevolverSetTurntable()); - OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whileHeld(new ChimneySet(-1.0)); + OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whileHeld(new ChimneyUnjam()); } /** diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java index d4ce18a..fd13f28 100644 --- a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java @@ -48,11 +48,11 @@ public DriveSlalom() { addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 66, 0)); addCommands(new TurnToHeading(20, 0.3, 50, TurnToHeading.Direction.RIGHT)); addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 20, 50)); - // COOL TURN ⬇️ + // COOL TURN addCommands(new TurnToHeading(1, 0.42, -45, TurnToHeading.Direction.LEFT)); addCommands(new TurnToHeading(1, 0.42, -135, TurnToHeading.Direction.LEFT)); addCommands(new TurnToHeading(1, 0.42, 100, TurnToHeading.Direction.LEFT)); - // COOL TURN ⬆️ + // COOL TURN addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 30, 135)); addCommands(new TurnToHeading(1, 0.3, -180, TurnToHeading.Direction.RIGHT)); addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 88, 180)); diff --git a/src/main/java/org/mayheminc/robot2020/commands/ChimneyUnjam.java b/src/main/java/org/mayheminc/robot2020/commands/ChimneyUnjam.java new file mode 100644 index 0000000..e14e663 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ChimneyUnjam.java @@ -0,0 +1,18 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.*; + +public class ChimneyUnjam extends SequentialCommandGroup { + /** Creates a new ChimneyUnjam. */ + public ChimneyUnjam() { + super(new ParallelCommandGroup(new RevolverSetTurntable(-0.5), new ChimneySet(-0.5))); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java index 9f73bb3..b188464 100644 --- a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java @@ -22,6 +22,10 @@ public enum DistanceUnits { ENCODER_TICKS, INCHES }; + public DriveStraightOnHeading(double arg_targetSpeed, double arg_distance) { + this(arg_targetSpeed, DistanceUnits.INCHES, arg_distance, RobotContainer.drive.getHeading()); + } + public DriveStraightOnHeading(double arg_targetSpeed, double arg_distance, double heading) { this(arg_targetSpeed, DistanceUnits.INCHES, arg_distance, heading); } diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java index da7b450..46a2e43 100644 --- a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -115,7 +115,11 @@ public void setPivot(Double b) { } public boolean isPivotAtPosition() { - return !m_isMoving; + // return !m_isMoving; + + // the intake motor is broken. + // the intake is now gravity fed + return true; } @Override @@ -150,13 +154,13 @@ private void updatePivotPower() { } void updateRollersPower() { - if (pivotTalon.getPosition() > (PIVOT_UP / 2.0)) { - // turn off the rollers automatically if the pivot is too high - rollersTalon.set(ControlMode.PercentOutput, 0.0); - } else { + // if (pivotTalon.getPosition() > (PIVOT_UP / 2.0)) { + // // turn off the rollers automatically if the pivot is too high + // rollersTalon.set(ControlMode.PercentOutput, 0.0); + // } else { // set the rollers as requested if the pivot is low enough rollersTalon.set(ControlMode.PercentOutput, m_desiredRollersPower); - } + // } } void updateSensorPosition() {