diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..84f9c6a --- /dev/null +++ b/.clang-format @@ -0,0 +1,2 @@ +BasedOnStyle: Google +IndentWidth: 4 diff --git a/.gitignnore b/.gitignnore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/.gitignnore @@ -0,0 +1 @@ +build diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..f8b92c3 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.gradle +build diff --git a/TODOLIST.md b/TODOLIST.md new file mode 100644 index 0000000..8d03936 --- /dev/null +++ b/TODOLIST.md @@ -0,0 +1,35 @@ +TODOLIST: + +1. Verify polarities at interface boundaries +Consult the coordinate frame I added to Constants.h. All functinality should be abstracted so as to appeal to this standard. + +Do this from the bottom-up, starting with swerve modules. + +All swerve modules when commanded +power, 0rad, should drive the robot in the +x direction, with speed sensor values increasing. + +When that is done, you adjust the negation of arguments in SwerveDrive::Drive to restore driving. + +When that is done, SwerveDrive::Drive should take x, y, and a bool indicating whether the x and y are given in world frame or robot frame. And the polarities must match. If this breaks the orientation of the Joysticks, you multiply the joystick values first, because this is not a swerve-drive concern, its a joystick concern. This ensures that the path following code can make use of Drive() in a sane way. + +2. Tune Odometry + +See github comment for this. Verify that this works by driving around all crazy. It won't work very well, (it never does), but it should WORK. It has to be passable for 10 seconds. Over that sort of time range, making smooth, medium speed manuevres, I'd expect at most 1-2 feet of error. + +3. Tune translational controllers. + +Setup an automode where the Robot drives a purely translational trajectory, in solely x, with 0 initial and end velocity. Begin by setting P=D=0. You can empirically derive an initial value for F using the theoretical max speed of the robot: + +1.0 = TRAJ_TRANSLATE_F * ROBOT_MAX_SPEED + +Your final value of F may be higher or lower than this. Tune F until the robot follows the path pretty well on it's own. The robot should arrive in about the right amount of time, but probably won't stop accurately. Then add in P. Then add in D (D should be negative, and very very small, if you use it at all). Remember that the purpose of adding D in is to allow a higher P, ending in faster convergence, so you can and should tune P too high and then add a small D. + +The usable gains for x and y should be the same. Test the tuned result on a path in both x and y. + +4. Tune rotational controllers. + +Do the same procedure as above on a path with only a rotational component. + +Then try a path with all components. + +5. Make some auto modes +ur done! See the stuff I checked in for roughly how I'd do it. diff --git a/build.gradle b/build.gradle index 3fac798..c15d177 100644 --- a/build.gradle +++ b/build.gradle @@ -76,6 +76,18 @@ model { wpi.cpp.vendor.cpp(it) wpi.cpp.deps.wpilib(it) } + pathTestProgram(NativeExecutableSpec) { + targetPlatform wpi.platforms.desktop + sources.cpp { + source { + srcDir 'src/pathtest/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + } + } + } } testSuites { frcUserProgramTest(GoogleTestTestSuiteSpec) { diff --git a/build/exe/frcUserProgram/debug/frcUserProgram b/build/exe/frcUserProgram/debug/frcUserProgram deleted file mode 100755 index f3189c2..0000000 Binary files a/build/exe/frcUserProgram/debug/frcUserProgram and /dev/null differ diff --git a/build/exe/frcUserProgram/debug/frcUserProgram.debug b/build/exe/frcUserProgram/debug/frcUserProgram.debug deleted file mode 100755 index fe48500..0000000 Binary files a/build/exe/frcUserProgram/debug/frcUserProgram.debug and /dev/null differ diff --git a/build/exe/frcUserProgram/release/frcUserProgram b/build/exe/frcUserProgram/release/frcUserProgram deleted file mode 100755 index a9b7cc7..0000000 Binary files a/build/exe/frcUserProgram/release/frcUserProgram and /dev/null differ diff --git a/build/exe/frcUserProgram/release/frcUserProgram.debug b/build/exe/frcUserProgram/release/frcUserProgram.debug deleted file mode 100755 index 1f7e067..0000000 Binary files a/build/exe/frcUserProgram/release/frcUserProgram.debug and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/12cvxskewty0b2tnckl6k1asx/Trajectory.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/12cvxskewty0b2tnckl6k1asx/Trajectory.o deleted file mode 100644 index fb33679..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/12cvxskewty0b2tnckl6k1asx/Trajectory.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/2d4xtn2rzx3pp7zim0aks1n8z/Channel.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/2d4xtn2rzx3pp7zim0aks1n8z/Channel.o deleted file mode 100644 index a031f0a..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/2d4xtn2rzx3pp7zim0aks1n8z/Channel.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/32eo27hkslc6k61fxm48owvd5/DataLogger.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/32eo27hkslc6k61fxm48owvd5/DataLogger.o deleted file mode 100644 index 091bbd8..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/32eo27hkslc6k61fxm48owvd5/DataLogger.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/7ofe8xjjzfh9r5xt9qht46m6f/WheelDrive.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/7ofe8xjjzfh9r5xt9qht46m6f/WheelDrive.o deleted file mode 100644 index 3a01217..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/7ofe8xjjzfh9r5xt9qht46m6f/WheelDrive.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/8mrstbn3b9vqn8dlra4iojn9w/AutoMode.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/8mrstbn3b9vqn8dlra4iojn9w/AutoMode.o deleted file mode 100644 index 45458c3..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/8mrstbn3b9vqn8dlra4iojn9w/AutoMode.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/9760xtgdau9jsoxxi90ozrpww/Robot.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/9760xtgdau9jsoxxi90ozrpww/Robot.o deleted file mode 100644 index a499c3b..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/9760xtgdau9jsoxxi90ozrpww/Robot.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/a6p299p5wr41guo83xc7lz2xf/SwerveDrive.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/a6p299p5wr41guo83xc7lz2xf/SwerveDrive.o deleted file mode 100644 index c49c676..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/a6p299p5wr41guo83xc7lz2xf/SwerveDrive.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/ad62nqwux8fjxvee1it7hnx3x/SwerveController.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/ad62nqwux8fjxvee1it7hnx3x/SwerveController.o deleted file mode 100644 index 14ada3c..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/ad62nqwux8fjxvee1it7hnx3x/SwerveController.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/al153h3x7u4s2d2pejopnu3r7/Climber.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/al153h3x7u4s2d2pejopnu3r7/Climber.o deleted file mode 100644 index ce5a3a8..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/al153h3x7u4s2d2pejopnu3r7/Climber.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/cvwvsen8q666l782ee6gpmhk5/Shooter.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/cvwvsen8q666l782ee6gpmhk5/Shooter.o deleted file mode 100644 index 56b2767..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/cvwvsen8q666l782ee6gpmhk5/Shooter.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/duga8cebqxgrmj4waqpnjghno/Limelight.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/duga8cebqxgrmj4waqpnjghno/Limelight.o deleted file mode 100644 index 6061c88..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/duga8cebqxgrmj4waqpnjghno/Limelight.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/eyoxex1ujygr70kntyhww47rg/Odometry.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/eyoxex1ujygr70kntyhww47rg/Odometry.o deleted file mode 100644 index ef772da..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/eyoxex1ujygr70kntyhww47rg/Odometry.o and /dev/null differ diff --git a/build/objs/frcUserProgram/debug/frcUserProgramCpp/hu1w5l5c20c5oie3dkbp0flf/Intake.o b/build/objs/frcUserProgram/debug/frcUserProgramCpp/hu1w5l5c20c5oie3dkbp0flf/Intake.o deleted file mode 100644 index c152a47..0000000 Binary files a/build/objs/frcUserProgram/debug/frcUserProgramCpp/hu1w5l5c20c5oie3dkbp0flf/Intake.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/12cvxskewty0b2tnckl6k1asx/Trajectory.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/12cvxskewty0b2tnckl6k1asx/Trajectory.o deleted file mode 100644 index c19f2ae..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/12cvxskewty0b2tnckl6k1asx/Trajectory.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/2d4xtn2rzx3pp7zim0aks1n8z/Channel.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/2d4xtn2rzx3pp7zim0aks1n8z/Channel.o deleted file mode 100644 index 52946af..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/2d4xtn2rzx3pp7zim0aks1n8z/Channel.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/32eo27hkslc6k61fxm48owvd5/DataLogger.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/32eo27hkslc6k61fxm48owvd5/DataLogger.o deleted file mode 100644 index 566bf84..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/32eo27hkslc6k61fxm48owvd5/DataLogger.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/7ofe8xjjzfh9r5xt9qht46m6f/WheelDrive.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/7ofe8xjjzfh9r5xt9qht46m6f/WheelDrive.o deleted file mode 100644 index 0ccd037..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/7ofe8xjjzfh9r5xt9qht46m6f/WheelDrive.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/8mrstbn3b9vqn8dlra4iojn9w/AutoMode.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/8mrstbn3b9vqn8dlra4iojn9w/AutoMode.o deleted file mode 100644 index 8624983..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/8mrstbn3b9vqn8dlra4iojn9w/AutoMode.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/9760xtgdau9jsoxxi90ozrpww/Robot.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/9760xtgdau9jsoxxi90ozrpww/Robot.o deleted file mode 100644 index b663c6f..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/9760xtgdau9jsoxxi90ozrpww/Robot.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/a6p299p5wr41guo83xc7lz2xf/SwerveDrive.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/a6p299p5wr41guo83xc7lz2xf/SwerveDrive.o deleted file mode 100644 index 4a5cd1a..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/a6p299p5wr41guo83xc7lz2xf/SwerveDrive.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/ad62nqwux8fjxvee1it7hnx3x/SwerveController.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/ad62nqwux8fjxvee1it7hnx3x/SwerveController.o deleted file mode 100644 index 1c1df6e..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/ad62nqwux8fjxvee1it7hnx3x/SwerveController.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/al153h3x7u4s2d2pejopnu3r7/Climber.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/al153h3x7u4s2d2pejopnu3r7/Climber.o deleted file mode 100644 index da3ae6f..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/al153h3x7u4s2d2pejopnu3r7/Climber.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/cvwvsen8q666l782ee6gpmhk5/Shooter.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/cvwvsen8q666l782ee6gpmhk5/Shooter.o deleted file mode 100644 index cbce9c2..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/cvwvsen8q666l782ee6gpmhk5/Shooter.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/duga8cebqxgrmj4waqpnjghno/Limelight.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/duga8cebqxgrmj4waqpnjghno/Limelight.o deleted file mode 100644 index fab31c1..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/duga8cebqxgrmj4waqpnjghno/Limelight.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/eyoxex1ujygr70kntyhww47rg/Odometry.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/eyoxex1ujygr70kntyhww47rg/Odometry.o deleted file mode 100644 index 2caa8e8..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/eyoxex1ujygr70kntyhww47rg/Odometry.o and /dev/null differ diff --git a/build/objs/frcUserProgram/release/frcUserProgramCpp/hu1w5l5c20c5oie3dkbp0flf/Intake.o b/build/objs/frcUserProgram/release/frcUserProgramCpp/hu1w5l5c20c5oie3dkbp0flf/Intake.o deleted file mode 100644 index 821a058..0000000 Binary files a/build/objs/frcUserProgram/release/frcUserProgramCpp/hu1w5l5c20c5oie3dkbp0flf/Intake.o and /dev/null differ diff --git a/build/tmp/compileFrcUserProgramDebugExecutableFrcUserProgramCpp/options.txt b/build/tmp/compileFrcUserProgramDebugExecutableFrcUserProgramCpp/options.txt deleted file mode 100644 index e989202..0000000 --- a/build/tmp/compileFrcUserProgramDebugExecutableFrcUserProgramCpp/options.txt +++ /dev/null @@ -1,83 +0,0 @@ --x -c++ --c --Wall --Wextra --std=c++17 --Wformat=2 --pedantic --Wno-psabi --Wno-unused-parameter --Wno-error=deprecated-declarations --fPIC --rdynamic --pthread --Og --g --nostdinc --I -/home/susan/SammyLogging/src/main/include --I -/home/susan/.gradle/caches/transforms-3/8c87b56b0ecd2defe52635c79f01e8e1/transformed/wpilibNewCommands-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/5e1f4de3a9a1449e6a39ad1a942acb9c/transformed/REVLib-cpp-2022.1.1-headers --I -/home/susan/.gradle/caches/transforms-3/a49f7d43b743c46872cab66ff31a7471/transformed/REVLib-driver-2022.1.1-headers --I -/home/susan/.gradle/caches/transforms-3/ba29cb9264109ae9ce10a3c2c4e7911e/transformed/wpiapi-cpp-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/c8928631f160af67ff1c04c7385076b9/transformed/api-cpp-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/a03dc906d0672b10588d230a11e608fa/transformed/cci-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/2283db808f306fb0fa86a48712306830/transformed/wpiapi-cpp-sim-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/e8a2836d3180043d91ed4daed1612e4d/transformed/api-cpp-sim-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/586d42abad815c0b0f34117fa93e20b0/transformed/cci-sim-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/e3e94461e07b8cb4cb778f297c838a6c/transformed/simTalonSRX-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/b3e54816efe38d5f889a3f9ea9330d16/transformed/simTalonFX-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/c80a43db59ea64498288dba883fe391e/transformed/simVictorSPX-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/4af49850607ce2da9cfc6e6ab7851162/transformed/simPigeonIMU-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/b9a9e42a33689df10221aaa788c97e85/transformed/simCANCoder-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/9e8a5a74c74c8235133be528f702b7f0/transformed/navx-cpp-4.0.442-headers --I -/home/susan/.gradle/caches/transforms-3/6ec6f73e6581f4bfe9dad7033f036ad9/transformed/wpilibc-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/dd1fda39118691c5bf59ed1d17658648/transformed/ntcore-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/bfd87a5cad0ec47eb318745d9dcf6c23/transformed/hal-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/ee5d1035c4a3f1f67e8aec81b66e32d0/transformed/wpimath-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/41871ba548c354018cd047bceee44bab/transformed/wpiutil-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/1008ab75f241c8086800b8f7095e3eea/transformed/netcomm-2022.4.0-headers --I -/home/susan/.gradle/caches/transforms-3/f9d1d4885992a7a8b89bf4c594996390/transformed/chipobject-2022.4.0-headers --I -/home/susan/.gradle/caches/transforms-3/bdeaa1aa50eae116dc4e43a94f3a65f6/transformed/visa-2022.4.0-headers --I -/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/4ffbd6d713a80732615525b599c34fbc/transformed/cscore-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/509d89d78b44e4903a3f7e7858891526/transformed/opencv-cpp-4.5.2-1-headers --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/lib/gcc/arm-frc2022-linux-gnueabi/7.3.0/include --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/lib/gcc/arm-frc2022-linux-gnueabi/7.3.0/include-fixed --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/include/c++/7.3.0 --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/include/c++/7.3.0/arm-frc2022-linux-gnueabi --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/include/c++/7.3.0/backward --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/include diff --git a/build/tmp/compileFrcUserProgramDebugExecutableFrcUserProgramCpp/output.txt b/build/tmp/compileFrcUserProgramDebugExecutableFrcUserProgramCpp/output.txt deleted file mode 100644 index 51dc764..0000000 --- a/build/tmp/compileFrcUserProgramDebugExecutableFrcUserProgramCpp/output.txt +++ /dev/null @@ -1,49 +0,0 @@ -See file:///home/susan/SammyLogging/build/tmp/compileFrcUserProgramDebugExecutableFrcUserProgramCpp/output.txt for all output for compileFrcUserProgramDebugExecutableFrcUserProgramCpp. -compiling Odometry.cpp successful. - -compiling AutoMode.cpp successful. - -compiling SwerveController.cpp successful. - -compiling DataLogger.cpp successful. - -compiling Channel.cpp successful. - -compiling Intake.cpp successful. - -compiling Limelight.cpp successful. - -compiling WheelDrive.cpp successful. - -compiling SwerveDrive.cpp successful. - -compiling Shooter.cpp successful. -/home/susan/SammyLogging/src/main/cpp/Shooter.cpp: In member function 'void Shooter::peekTurret(double, double)': -/home/susan/SammyLogging/src/main/cpp/Shooter.cpp:459:12: warning: unused variable 'POV_deg' [-Wunused-variable] - double POV_deg = POV>180? POV-360:POV; - ^~~~~~~ - -compiling Robot.cpp successful. -/home/susan/SammyLogging/src/main/cpp/Robot.cpp: In member function 'virtual void Robot::RobotInit()': -/home/susan/SammyLogging/src/main/cpp/Robot.cpp:24:31: warning: 'static frc::CameraServer* frc::CameraServer::GetInstance()' is deprecated: Use static methods [-Wdeprecated-declarations] - camera = frc::CameraServer::GetInstance()->StartAutomaticCapture(); - ^~~~~~~~~~~ -In file included from /home/susan/SammyLogging/src/main/include/Robot.h:20:0, - from /home/susan/SammyLogging/src/main/cpp/Robot.cpp:1: -/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/cameraserver/CameraServer.h:37:24: note: declared here - static CameraServer* GetInstance(); - ^~~~~~~~~~~ -/home/susan/SammyLogging/src/main/cpp/Robot.cpp:24:43: warning: 'static frc::CameraServer* frc::CameraServer::GetInstance()' is deprecated: Use static methods [-Wdeprecated-declarations] - camera = frc::CameraServer::GetInstance()->StartAutomaticCapture(); - ^ -In file included from /home/susan/SammyLogging/src/main/include/Robot.h:20:0, - from /home/susan/SammyLogging/src/main/cpp/Robot.cpp:1: -/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/cameraserver/CameraServer.h:37:24: note: declared here - static CameraServer* GetInstance(); - ^~~~~~~~~~~ - -compiling Trajectory.cpp successful. - -compiling Climber.cpp successful. - -Finished compileFrcUserProgramDebugExecutableFrcUserProgramCpp, see full log file:///home/susan/SammyLogging/build/tmp/compileFrcUserProgramDebugExecutableFrcUserProgramCpp/output.txt. diff --git a/build/tmp/compileFrcUserProgramReleaseExecutableFrcUserProgramCpp/options.txt b/build/tmp/compileFrcUserProgramReleaseExecutableFrcUserProgramCpp/options.txt deleted file mode 100644 index 289401e..0000000 --- a/build/tmp/compileFrcUserProgramReleaseExecutableFrcUserProgramCpp/options.txt +++ /dev/null @@ -1,82 +0,0 @@ --x -c++ --c --Wall --Wextra --std=c++17 --Wformat=2 --pedantic --Wno-psabi --Wno-unused-parameter --Wno-error=deprecated-declarations --fPIC --rdynamic --pthread --O2 --nostdinc --I -/home/susan/SammyLogging/src/main/include --I -/home/susan/.gradle/caches/transforms-3/8c87b56b0ecd2defe52635c79f01e8e1/transformed/wpilibNewCommands-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/5e1f4de3a9a1449e6a39ad1a942acb9c/transformed/REVLib-cpp-2022.1.1-headers --I -/home/susan/.gradle/caches/transforms-3/a49f7d43b743c46872cab66ff31a7471/transformed/REVLib-driver-2022.1.1-headers --I -/home/susan/.gradle/caches/transforms-3/ba29cb9264109ae9ce10a3c2c4e7911e/transformed/wpiapi-cpp-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/c8928631f160af67ff1c04c7385076b9/transformed/api-cpp-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/a03dc906d0672b10588d230a11e608fa/transformed/cci-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/2283db808f306fb0fa86a48712306830/transformed/wpiapi-cpp-sim-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/e8a2836d3180043d91ed4daed1612e4d/transformed/api-cpp-sim-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/586d42abad815c0b0f34117fa93e20b0/transformed/cci-sim-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/e3e94461e07b8cb4cb778f297c838a6c/transformed/simTalonSRX-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/b3e54816efe38d5f889a3f9ea9330d16/transformed/simTalonFX-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/c80a43db59ea64498288dba883fe391e/transformed/simVictorSPX-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/4af49850607ce2da9cfc6e6ab7851162/transformed/simPigeonIMU-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/b9a9e42a33689df10221aaa788c97e85/transformed/simCANCoder-5.20.2-headers --I -/home/susan/.gradle/caches/transforms-3/9e8a5a74c74c8235133be528f702b7f0/transformed/navx-cpp-4.0.442-headers --I -/home/susan/.gradle/caches/transforms-3/6ec6f73e6581f4bfe9dad7033f036ad9/transformed/wpilibc-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/dd1fda39118691c5bf59ed1d17658648/transformed/ntcore-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/bfd87a5cad0ec47eb318745d9dcf6c23/transformed/hal-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/ee5d1035c4a3f1f67e8aec81b66e32d0/transformed/wpimath-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/41871ba548c354018cd047bceee44bab/transformed/wpiutil-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/1008ab75f241c8086800b8f7095e3eea/transformed/netcomm-2022.4.0-headers --I -/home/susan/.gradle/caches/transforms-3/f9d1d4885992a7a8b89bf4c594996390/transformed/chipobject-2022.4.0-headers --I -/home/susan/.gradle/caches/transforms-3/bdeaa1aa50eae116dc4e43a94f3a65f6/transformed/visa-2022.4.0-headers --I -/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/4ffbd6d713a80732615525b599c34fbc/transformed/cscore-cpp-2022.3.1-headers --I -/home/susan/.gradle/caches/transforms-3/509d89d78b44e4903a3f7e7858891526/transformed/opencv-cpp-4.5.2-1-headers --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/lib/gcc/arm-frc2022-linux-gnueabi/7.3.0/include --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/lib/gcc/arm-frc2022-linux-gnueabi/7.3.0/include-fixed --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/include/c++/7.3.0 --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/include/c++/7.3.0/arm-frc2022-linux-gnueabi --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/include/c++/7.3.0/backward --isystem -/home/susan/wpilib/2022/roborio/arm-frc2022-linux-gnueabi/usr/include diff --git a/build/tmp/compileFrcUserProgramReleaseExecutableFrcUserProgramCpp/output.txt b/build/tmp/compileFrcUserProgramReleaseExecutableFrcUserProgramCpp/output.txt deleted file mode 100644 index 4965825..0000000 --- a/build/tmp/compileFrcUserProgramReleaseExecutableFrcUserProgramCpp/output.txt +++ /dev/null @@ -1,49 +0,0 @@ -See file:///home/susan/SammyLogging/build/tmp/compileFrcUserProgramReleaseExecutableFrcUserProgramCpp/output.txt for all output for compileFrcUserProgramReleaseExecutableFrcUserProgramCpp. -compiling Odometry.cpp successful. - -compiling Intake.cpp successful. - -compiling Trajectory.cpp successful. - -compiling DataLogger.cpp successful. - -compiling SwerveController.cpp successful. - -compiling AutoMode.cpp successful. - -compiling Channel.cpp successful. - -compiling Limelight.cpp successful. - -compiling Climber.cpp successful. - -compiling WheelDrive.cpp successful. - -compiling SwerveDrive.cpp successful. - -compiling Robot.cpp successful. -/home/susan/SammyLogging/src/main/cpp/Robot.cpp: In member function 'virtual void Robot::RobotInit()': -/home/susan/SammyLogging/src/main/cpp/Robot.cpp:24:31: warning: 'static frc::CameraServer* frc::CameraServer::GetInstance()' is deprecated: Use static methods [-Wdeprecated-declarations] - camera = frc::CameraServer::GetInstance()->StartAutomaticCapture(); - ^~~~~~~~~~~ -In file included from /home/susan/SammyLogging/src/main/include/Robot.h:20:0, - from /home/susan/SammyLogging/src/main/cpp/Robot.cpp:1: -/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/cameraserver/CameraServer.h:37:24: note: declared here - static CameraServer* GetInstance(); - ^~~~~~~~~~~ -/home/susan/SammyLogging/src/main/cpp/Robot.cpp:24:43: warning: 'static frc::CameraServer* frc::CameraServer::GetInstance()' is deprecated: Use static methods [-Wdeprecated-declarations] - camera = frc::CameraServer::GetInstance()->StartAutomaticCapture(); - ^ -In file included from /home/susan/SammyLogging/src/main/include/Robot.h:20:0, - from /home/susan/SammyLogging/src/main/cpp/Robot.cpp:1: -/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/cameraserver/CameraServer.h:37:24: note: declared here - static CameraServer* GetInstance(); - ^~~~~~~~~~~ - -compiling Shooter.cpp successful. -/home/susan/SammyLogging/src/main/cpp/Shooter.cpp: In member function 'void Shooter::peekTurret(double, double)': -/home/susan/SammyLogging/src/main/cpp/Shooter.cpp:459:12: warning: unused variable 'POV_deg' [-Wunused-variable] - double POV_deg = POV>180? POV-360:POV; - ^~~~~~~ - -Finished compileFrcUserProgramReleaseExecutableFrcUserProgramCpp, see full log file:///home/susan/SammyLogging/build/tmp/compileFrcUserProgramReleaseExecutableFrcUserProgramCpp/output.txt. diff --git a/build/tmp/linkFrcUserProgramDebugExecutable/options.txt b/build/tmp/linkFrcUserProgramDebugExecutable/options.txt deleted file mode 100644 index 5a86b68..0000000 --- a/build/tmp/linkFrcUserProgramDebugExecutable/options.txt +++ /dev/null @@ -1,52 +0,0 @@ --o -/home/susan/SammyLogging/build/exe/frcUserProgram/debug/frcUserProgram -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/al153h3x7u4s2d2pejopnu3r7/Climber.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/12cvxskewty0b2tnckl6k1asx/Trajectory.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/2d4xtn2rzx3pp7zim0aks1n8z/Channel.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/duga8cebqxgrmj4waqpnjghno/Limelight.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/9760xtgdau9jsoxxi90ozrpww/Robot.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/7ofe8xjjzfh9r5xt9qht46m6f/WheelDrive.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/32eo27hkslc6k61fxm48owvd5/DataLogger.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/hu1w5l5c20c5oie3dkbp0flf/Intake.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/eyoxex1ujygr70kntyhww47rg/Odometry.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/8mrstbn3b9vqn8dlra4iojn9w/AutoMode.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/a6p299p5wr41guo83xc7lz2xf/SwerveDrive.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/cvwvsen8q666l782ee6gpmhk5/Shooter.o -/home/susan/SammyLogging/build/objs/frcUserProgram/debug/frcUserProgramCpp/ad62nqwux8fjxvee1it7hnx3x/SwerveController.o -/home/susan/.gradle/caches/transforms-3/36f54b3d1a187a267f58571488faa2a4/transformed/wpilibNewCommands-cpp-2022.3.1-linuxathenadebug/linux/athena/shared/libwpilibNewCommandsd.so -/home/susan/.gradle/caches/transforms-3/4a683ab59a1d12a147ca80ebc025f0d5/transformed/REVLib-cpp-2022.1.1-linuxathenastaticdebug/linux/athena/static/libREVLib.a -/home/susan/.gradle/caches/transforms-3/45a84a662cfb2da43230c5f8bbce4716/transformed/REVLib-driver-2022.1.1-linuxathenastaticdebug/linux/athena/static/libREVLibDriver.a -/home/susan/.gradle/caches/transforms-3/69fbbfa694f18fe8cd05b84f0b0d1eef/transformed/wpiapi-cpp-5.20.2-linuxathenadebug/linux/athena/shared/libCTRE_Phoenix_WPI.so -/home/susan/.gradle/caches/transforms-3/efa5cae3e55ce7f2c859a62287a822f5/transformed/api-cpp-5.20.2-linuxathenadebug/linux/athena/shared/libCTRE_Phoenix.so -/home/susan/.gradle/caches/transforms-3/063da60f95356670182c443c241f6221/transformed/cci-5.20.2-linuxathenadebug/linux/athena/shared/libCTRE_PhoenixCCI.so -/home/susan/.gradle/caches/transforms-3/94092340d2d1d96a1b18b63d6a3d33b4/transformed/navx-cpp-4.0.442-linuxathenastaticdebug/linux/athena/static/libnavx_frcd.a -/home/susan/.gradle/caches/transforms-3/6abe3fbb6c78402c5c204fd790eb67ba/transformed/wpilibc-cpp-2022.3.1-linuxathenadebug/linux/athena/shared/libwpilibcd.so -/home/susan/.gradle/caches/transforms-3/025928c4500d7eff8c6ddb14bf621708/transformed/ntcore-cpp-2022.3.1-linuxathenadebug/linux/athena/shared/libntcored.so -/home/susan/.gradle/caches/transforms-3/3714d9c4a4971d6a09617f056a2135d8/transformed/hal-cpp-2022.3.1-linuxathenadebug/linux/athena/shared/libwpiHald.so -/home/susan/.gradle/caches/transforms-3/e8c8de81c4e7c40b842724ff6f352378/transformed/wpimath-cpp-2022.3.1-linuxathenadebug/linux/athena/shared/libwpimathd.so -/home/susan/.gradle/caches/transforms-3/ff793d80f358a2be677bda9a3fe17a02/transformed/wpiutil-cpp-2022.3.1-linuxathenadebug/linux/athena/shared/libwpiutild.so -/home/susan/.gradle/caches/transforms-3/196b10f418510fd256eee369e20ab94c/transformed/netcomm-2022.4.0-linuxathenadebug/linux/athena/shared/libFRC_NetworkCommunication.so.22.0.0 -/home/susan/.gradle/caches/transforms-3/f2c598c7328ee38ddcc0c7aedcb498b9/transformed/chipobject-2022.4.0-linuxathenadebug/linux/athena/shared/libRoboRIO_FRC_ChipObject.so.22.0.0 -/home/susan/.gradle/caches/transforms-3/fabc16dcd0381671fbf4a5b5500623c7/transformed/visa-2022.4.0-linuxathenadebug/linux/athena/shared/libvisa.so.21.0.0 -/home/susan/.gradle/caches/transforms-3/52ca36e5b09ad6d0b88c8fba8bf8e8ee/transformed/runtime-2022.4.0-linuxathenadebug/linux/athena/shared/libembcanshim.so -/home/susan/.gradle/caches/transforms-3/52ca36e5b09ad6d0b88c8fba8bf8e8ee/transformed/runtime-2022.4.0-linuxathenadebug/linux/athena/shared/libfpgalvshim.so -/home/susan/.gradle/caches/transforms-3/0b721204f4c8802eb4032b6727718e65/transformed/cameraserver-cpp-2022.3.1-linuxathenadebug/linux/athena/shared/libcameraserverd.so -/home/susan/.gradle/caches/transforms-3/d76863f3cb1071fa3ef95cdb1574e3b4/transformed/cscore-cpp-2022.3.1-linuxathenadebug/linux/athena/shared/libcscored.so -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_gapid.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_imgcodecsd.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_cored.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_stitchingd.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_imgprocd.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_videoiod.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_objdetectd.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_photod.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_mld.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_highguid.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_features2dd.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_videod.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_calib3dd.so.4.5 -/home/susan/.gradle/caches/transforms-3/9fa8f0c9317b707e3d0376de8d881794/transformed/opencv-cpp-4.5.2-1-linuxathenadebug/linux/athena/shared/libopencv_flannd.so.4.5 --rdynamic --pthread --ldl --latomic diff --git a/build/tmp/linkFrcUserProgramDebugExecutable/output.txt b/build/tmp/linkFrcUserProgramDebugExecutable/output.txt deleted file mode 100644 index f3e9581..0000000 --- a/build/tmp/linkFrcUserProgramDebugExecutable/output.txt +++ /dev/null @@ -1,4 +0,0 @@ -See file:///home/susan/SammyLogging/build/tmp/linkFrcUserProgramDebugExecutable/output.txt for all output for linkFrcUserProgramDebugExecutable. -linking frcUserProgram successful. - -Finished linkFrcUserProgramDebugExecutable, see full log file:///home/susan/SammyLogging/build/tmp/linkFrcUserProgramDebugExecutable/output.txt. diff --git a/build/tmp/linkFrcUserProgramReleaseExecutable/options.txt b/build/tmp/linkFrcUserProgramReleaseExecutable/options.txt deleted file mode 100644 index df83afa..0000000 --- a/build/tmp/linkFrcUserProgramReleaseExecutable/options.txt +++ /dev/null @@ -1,52 +0,0 @@ --o -/home/susan/SammyLogging/build/exe/frcUserProgram/release/frcUserProgram -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/al153h3x7u4s2d2pejopnu3r7/Climber.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/12cvxskewty0b2tnckl6k1asx/Trajectory.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/2d4xtn2rzx3pp7zim0aks1n8z/Channel.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/duga8cebqxgrmj4waqpnjghno/Limelight.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/9760xtgdau9jsoxxi90ozrpww/Robot.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/7ofe8xjjzfh9r5xt9qht46m6f/WheelDrive.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/32eo27hkslc6k61fxm48owvd5/DataLogger.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/hu1w5l5c20c5oie3dkbp0flf/Intake.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/eyoxex1ujygr70kntyhww47rg/Odometry.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/8mrstbn3b9vqn8dlra4iojn9w/AutoMode.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/a6p299p5wr41guo83xc7lz2xf/SwerveDrive.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/cvwvsen8q666l782ee6gpmhk5/Shooter.o -/home/susan/SammyLogging/build/objs/frcUserProgram/release/frcUserProgramCpp/ad62nqwux8fjxvee1it7hnx3x/SwerveController.o -/home/susan/.gradle/caches/transforms-3/3c843b47031719c5e088c52f1c0c85cb/transformed/wpilibNewCommands-cpp-2022.3.1-linuxathena/linux/athena/shared/libwpilibNewCommands.so -/home/susan/.gradle/caches/transforms-3/524da20a3554594a910f236ed8ba04d5/transformed/REVLib-cpp-2022.1.1-linuxathenastatic/linux/athena/static/libREVLib.a -/home/susan/.gradle/caches/transforms-3/8050422e2994ee92ade31cd13a1fdcf2/transformed/REVLib-driver-2022.1.1-linuxathenastatic/linux/athena/static/libREVLibDriver.a -/home/susan/.gradle/caches/transforms-3/59a4123ff30ecf4b3f692938711da19b/transformed/wpiapi-cpp-5.20.2-linuxathena/linux/athena/shared/libCTRE_Phoenix_WPI.so -/home/susan/.gradle/caches/transforms-3/fcded5b2b244f39f97df08ab2d9e8e6b/transformed/api-cpp-5.20.2-linuxathena/linux/athena/shared/libCTRE_Phoenix.so -/home/susan/.gradle/caches/transforms-3/98bfdc4663f5bb88c406eefb9e6a421a/transformed/cci-5.20.2-linuxathena/linux/athena/shared/libCTRE_PhoenixCCI.so -/home/susan/.gradle/caches/transforms-3/2193907ad677d736ac8e37338c85dcb1/transformed/navx-cpp-4.0.442-linuxathenastatic/linux/athena/static/libnavx_frc.a -/home/susan/.gradle/caches/transforms-3/f99d90d2e6bfbce489d67e7958b37f8b/transformed/wpilibc-cpp-2022.3.1-linuxathena/linux/athena/shared/libwpilibc.so -/home/susan/.gradle/caches/transforms-3/db9f716469148e5789a0618a5797dcd2/transformed/ntcore-cpp-2022.3.1-linuxathena/linux/athena/shared/libntcore.so -/home/susan/.gradle/caches/transforms-3/0690e0afa3e9d58bf188a8bfa97a72dc/transformed/hal-cpp-2022.3.1-linuxathena/linux/athena/shared/libwpiHal.so -/home/susan/.gradle/caches/transforms-3/2866bce98744872ef83dd786af76980e/transformed/wpimath-cpp-2022.3.1-linuxathena/linux/athena/shared/libwpimath.so -/home/susan/.gradle/caches/transforms-3/650b5c172d0a3b319d68a07f0d2e1161/transformed/wpiutil-cpp-2022.3.1-linuxathena/linux/athena/shared/libwpiutil.so -/home/susan/.gradle/caches/transforms-3/0ed3916745141228e717024557c560d0/transformed/netcomm-2022.4.0-linuxathena/linux/athena/shared/libFRC_NetworkCommunication.so.22.0.0 -/home/susan/.gradle/caches/transforms-3/c91191fe36001977227bec0af2f0f411/transformed/chipobject-2022.4.0-linuxathena/linux/athena/shared/libRoboRIO_FRC_ChipObject.so.22.0.0 -/home/susan/.gradle/caches/transforms-3/765dfa5717a514bd73254a80f5a4ce0c/transformed/visa-2022.4.0-linuxathena/linux/athena/shared/libvisa.so.21.0.0 -/home/susan/.gradle/caches/transforms-3/187e18f3838e6d97840f164d517cef22/transformed/runtime-2022.4.0-linuxathena/linux/athena/shared/libembcanshim.so -/home/susan/.gradle/caches/transforms-3/187e18f3838e6d97840f164d517cef22/transformed/runtime-2022.4.0-linuxathena/linux/athena/shared/libfpgalvshim.so -/home/susan/.gradle/caches/transforms-3/539d9c7b36c70e70a38033e62ecf08d2/transformed/cameraserver-cpp-2022.3.1-linuxathena/linux/athena/shared/libcameraserver.so -/home/susan/.gradle/caches/transforms-3/354a3d31ea0522e8f078c6458e924a9d/transformed/cscore-cpp-2022.3.1-linuxathena/linux/athena/shared/libcscore.so -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_imgproc.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_core.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_videoio.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_imgcodecs.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_ml.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_features2d.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_photo.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_stitching.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_flann.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_video.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_highgui.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_objdetect.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_calib3d.so.4.5 -/home/susan/.gradle/caches/transforms-3/72220dc2cd73d9c0772e9d913fe8b455/transformed/opencv-cpp-4.5.2-1-linuxathena/linux/athena/shared/libopencv_gapi.so.4.5 --rdynamic --pthread --ldl --latomic diff --git a/build/tmp/linkFrcUserProgramReleaseExecutable/output.txt b/build/tmp/linkFrcUserProgramReleaseExecutable/output.txt deleted file mode 100644 index 4f3b43b..0000000 --- a/build/tmp/linkFrcUserProgramReleaseExecutable/output.txt +++ /dev/null @@ -1,4 +0,0 @@ -See file:///home/susan/SammyLogging/build/tmp/linkFrcUserProgramReleaseExecutable/output.txt for all output for linkFrcUserProgramReleaseExecutable. -linking frcUserProgram successful. - -Finished linkFrcUserProgramReleaseExecutable, see full log file:///home/susan/SammyLogging/build/tmp/linkFrcUserProgramReleaseExecutable/output.txt. diff --git a/build/vscodeconfig.json b/build/vscodeconfig.json deleted file mode 100644 index cc8fe9f..0000000 --- a/build/vscodeconfig.json +++ /dev/null @@ -1,469 +0,0 @@ -[ - { - "name": "linuxathena", - "architecture": "arm-v7", - "operatingSystem": "linux", - "flavor": "default", - "buildType": "debug", - "cppPath": "/home/susan/wpilib/2022/roborio/bin/arm-frc2022-linux-gnueabi-g++", - "cPath": "/home/susan/wpilib/2022/roborio/bin/arm-frc2022-linux-gnueabi-gcc", - "msvc": false, - "gcc": true, - "systemCppMacros": [], - "systemCppArgs": [], - "systemCMacros": [], - "systemCArgs": [], - "allLibFiles": [ - "/home/susan/.gradle/caches/transforms-3/8c87b56b0ecd2defe52635c79f01e8e1/transformed/wpilibNewCommands-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/5e1f4de3a9a1449e6a39ad1a942acb9c/transformed/REVLib-cpp-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/a49f7d43b743c46872cab66ff31a7471/transformed/REVLib-driver-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ba29cb9264109ae9ce10a3c2c4e7911e/transformed/wpiapi-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c8928631f160af67ff1c04c7385076b9/transformed/api-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/a03dc906d0672b10588d230a11e608fa/transformed/cci-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/2283db808f306fb0fa86a48712306830/transformed/wpiapi-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e8a2836d3180043d91ed4daed1612e4d/transformed/api-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/586d42abad815c0b0f34117fa93e20b0/transformed/cci-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e3e94461e07b8cb4cb778f297c838a6c/transformed/simTalonSRX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b3e54816efe38d5f889a3f9ea9330d16/transformed/simTalonFX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c80a43db59ea64498288dba883fe391e/transformed/simVictorSPX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/4af49850607ce2da9cfc6e6ab7851162/transformed/simPigeonIMU-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b9a9e42a33689df10221aaa788c97e85/transformed/simCANCoder-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/9e8a5a74c74c8235133be528f702b7f0/transformed/navx-cpp-4.0.442-headers/", - "/home/susan/.gradle/caches/transforms-3/6ec6f73e6581f4bfe9dad7033f036ad9/transformed/wpilibc-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/dd1fda39118691c5bf59ed1d17658648/transformed/ntcore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/bfd87a5cad0ec47eb318745d9dcf6c23/transformed/hal-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ee5d1035c4a3f1f67e8aec81b66e32d0/transformed/wpimath-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/41871ba548c354018cd047bceee44bab/transformed/wpiutil-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/1008ab75f241c8086800b8f7095e3eea/transformed/netcomm-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/f9d1d4885992a7a8b89bf4c594996390/transformed/chipobject-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/bdeaa1aa50eae116dc4e43a94f3a65f6/transformed/visa-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/4ffbd6d713a80732615525b599c34fbc/transformed/cscore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/509d89d78b44e4903a3f7e7858891526/transformed/opencv-cpp-4.5.2-1-headers/", - "/home/susan/SammyLogging/src/main/include/", - "/home/susan/.gradle/caches/transforms-3/a92563cc976ad00d7462a3d2072c1f71/transformed/wpilibNewCommands-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/ecc71bf6213e16f4067ad0977709a6ec/transformed/navx-cpp-4.0.442-sources/", - "/home/susan/.gradle/caches/transforms-3/608adad05b9f8ff2c94b10a2eb62ab21/transformed/wpilibc-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/15f7cf8ecf12f549e1556423b5d67945/transformed/ntcore-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/cd1cdce935056caf6ce35a26aca97513/transformed/hal-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/284b17bbeb445b40ad36c9fca15aa2d3/transformed/wpimath-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/d870fcde75bdcfa583e5698baaf46ba5/transformed/wpiutil-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/93ec4944ee5b9d314a0f3f2b9d73996b/transformed/cameraserver-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/bdbf41924a0dc282b242fccac16b8e11/transformed/cscore-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/75aacf4b83742d44efabec092f799296/transformed/opencv-cpp-4.5.2-1-sources/" - ], - "allLibSources": [ - "/home/susan/.gradle/caches/transforms-3/a92563cc976ad00d7462a3d2072c1f71/transformed/wpilibNewCommands-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/ecc71bf6213e16f4067ad0977709a6ec/transformed/navx-cpp-4.0.442-sources/", - "/home/susan/.gradle/caches/transforms-3/608adad05b9f8ff2c94b10a2eb62ab21/transformed/wpilibc-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/15f7cf8ecf12f549e1556423b5d67945/transformed/ntcore-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/cd1cdce935056caf6ce35a26aca97513/transformed/hal-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/284b17bbeb445b40ad36c9fca15aa2d3/transformed/wpimath-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/d870fcde75bdcfa583e5698baaf46ba5/transformed/wpiutil-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/93ec4944ee5b9d314a0f3f2b9d73996b/transformed/cameraserver-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/bdbf41924a0dc282b242fccac16b8e11/transformed/cscore-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/75aacf4b83742d44efabec092f799296/transformed/opencv-cpp-4.5.2-1-sources/" - ], - "allLibHeaders": [ - "/home/susan/.gradle/caches/transforms-3/8c87b56b0ecd2defe52635c79f01e8e1/transformed/wpilibNewCommands-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/5e1f4de3a9a1449e6a39ad1a942acb9c/transformed/REVLib-cpp-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/a49f7d43b743c46872cab66ff31a7471/transformed/REVLib-driver-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ba29cb9264109ae9ce10a3c2c4e7911e/transformed/wpiapi-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c8928631f160af67ff1c04c7385076b9/transformed/api-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/a03dc906d0672b10588d230a11e608fa/transformed/cci-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/2283db808f306fb0fa86a48712306830/transformed/wpiapi-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e8a2836d3180043d91ed4daed1612e4d/transformed/api-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/586d42abad815c0b0f34117fa93e20b0/transformed/cci-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e3e94461e07b8cb4cb778f297c838a6c/transformed/simTalonSRX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b3e54816efe38d5f889a3f9ea9330d16/transformed/simTalonFX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c80a43db59ea64498288dba883fe391e/transformed/simVictorSPX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/4af49850607ce2da9cfc6e6ab7851162/transformed/simPigeonIMU-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b9a9e42a33689df10221aaa788c97e85/transformed/simCANCoder-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/9e8a5a74c74c8235133be528f702b7f0/transformed/navx-cpp-4.0.442-headers/", - "/home/susan/.gradle/caches/transforms-3/6ec6f73e6581f4bfe9dad7033f036ad9/transformed/wpilibc-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/dd1fda39118691c5bf59ed1d17658648/transformed/ntcore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/bfd87a5cad0ec47eb318745d9dcf6c23/transformed/hal-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ee5d1035c4a3f1f67e8aec81b66e32d0/transformed/wpimath-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/41871ba548c354018cd047bceee44bab/transformed/wpiutil-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/1008ab75f241c8086800b8f7095e3eea/transformed/netcomm-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/f9d1d4885992a7a8b89bf4c594996390/transformed/chipobject-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/bdeaa1aa50eae116dc4e43a94f3a65f6/transformed/visa-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/4ffbd6d713a80732615525b599c34fbc/transformed/cscore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/509d89d78b44e4903a3f7e7858891526/transformed/opencv-cpp-4.5.2-1-headers/", - "/home/susan/SammyLogging/src/main/include/" - ], - "binaries": [ - { - "componentName": "frcUserProgram", - "sourceSets": [ - { - "source": { - "srcDirs": [ - "/home/susan/SammyLogging/src/main/cpp/" - ], - "includes": [ - "**/*.cpp", - "**/*.cc" - ], - "excludes": [] - }, - "exportedHeaders": { - "srcDirs": [ - "/home/susan/SammyLogging/src/main/include/" - ], - "includes": [], - "excludes": [] - }, - "cpp": true, - "args": [ - "-Wall", - "-Wextra", - "-std=c++17", - "-Wformat=2", - "-pedantic", - "-Wno-psabi", - "-Wno-unused-parameter", - "-Wno-error=deprecated-declarations", - "-fPIC", - "-rdynamic", - "-pthread", - "-Og", - "-g" - ], - "macros": [] - } - ], - "libHeaders": [ - "/home/susan/.gradle/caches/transforms-3/8c87b56b0ecd2defe52635c79f01e8e1/transformed/wpilibNewCommands-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/5e1f4de3a9a1449e6a39ad1a942acb9c/transformed/REVLib-cpp-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/a49f7d43b743c46872cab66ff31a7471/transformed/REVLib-driver-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ba29cb9264109ae9ce10a3c2c4e7911e/transformed/wpiapi-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c8928631f160af67ff1c04c7385076b9/transformed/api-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/a03dc906d0672b10588d230a11e608fa/transformed/cci-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/2283db808f306fb0fa86a48712306830/transformed/wpiapi-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e8a2836d3180043d91ed4daed1612e4d/transformed/api-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/586d42abad815c0b0f34117fa93e20b0/transformed/cci-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e3e94461e07b8cb4cb778f297c838a6c/transformed/simTalonSRX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b3e54816efe38d5f889a3f9ea9330d16/transformed/simTalonFX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c80a43db59ea64498288dba883fe391e/transformed/simVictorSPX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/4af49850607ce2da9cfc6e6ab7851162/transformed/simPigeonIMU-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b9a9e42a33689df10221aaa788c97e85/transformed/simCANCoder-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/9e8a5a74c74c8235133be528f702b7f0/transformed/navx-cpp-4.0.442-headers/", - "/home/susan/.gradle/caches/transforms-3/6ec6f73e6581f4bfe9dad7033f036ad9/transformed/wpilibc-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/dd1fda39118691c5bf59ed1d17658648/transformed/ntcore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/bfd87a5cad0ec47eb318745d9dcf6c23/transformed/hal-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ee5d1035c4a3f1f67e8aec81b66e32d0/transformed/wpimath-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/41871ba548c354018cd047bceee44bab/transformed/wpiutil-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/1008ab75f241c8086800b8f7095e3eea/transformed/netcomm-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/f9d1d4885992a7a8b89bf4c594996390/transformed/chipobject-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/bdeaa1aa50eae116dc4e43a94f3a65f6/transformed/visa-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/4ffbd6d713a80732615525b599c34fbc/transformed/cscore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/509d89d78b44e4903a3f7e7858891526/transformed/opencv-cpp-4.5.2-1-headers/", - "/home/susan/SammyLogging/src/main/include/" - ], - "sharedLibrary": false, - "executable": true - } - ], - "sourceBinaries": [ - { - "source": { - "srcDirs": [ - "/home/susan/SammyLogging/src/main/cpp/" - ], - "includes": [ - "**/*.cpp", - "**/*.cc" - ], - "excludes": [] - }, - "componentName": "frcUserProgram", - "cpp": true, - "args": [ - "-Wall", - "-Wextra", - "-std=c++17", - "-Wformat=2", - "-pedantic", - "-Wno-psabi", - "-Wno-unused-parameter", - "-Wno-error=deprecated-declarations", - "-fPIC", - "-rdynamic", - "-pthread", - "-Og", - "-g" - ], - "macros": [], - "sharedLibrary": false, - "executable": true - }, - { - "source": { - "srcDirs": [ - "/home/susan/SammyLogging/src/main/include/" - ], - "includes": [], - "excludes": [] - }, - "componentName": "frcUserProgram", - "cpp": true, - "args": [ - "-Wall", - "-Wextra", - "-std=c++17", - "-Wformat=2", - "-pedantic", - "-Wno-psabi", - "-Wno-unused-parameter", - "-Wno-error=deprecated-declarations", - "-fPIC", - "-rdynamic", - "-pthread", - "-Og", - "-g" - ], - "macros": [], - "sharedLibrary": false, - "executable": true - } - ], - "nameBinaryMap": { - "frcUserProgram": 0 - } - }, - { - "name": "linuxathena", - "architecture": "arm-v7", - "operatingSystem": "linux", - "flavor": "default", - "buildType": "release", - "cppPath": "/home/susan/wpilib/2022/roborio/bin/arm-frc2022-linux-gnueabi-g++", - "cPath": "/home/susan/wpilib/2022/roborio/bin/arm-frc2022-linux-gnueabi-gcc", - "msvc": false, - "gcc": true, - "systemCppMacros": [], - "systemCppArgs": [], - "systemCMacros": [], - "systemCArgs": [], - "allLibFiles": [ - "/home/susan/.gradle/caches/transforms-3/8c87b56b0ecd2defe52635c79f01e8e1/transformed/wpilibNewCommands-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/5e1f4de3a9a1449e6a39ad1a942acb9c/transformed/REVLib-cpp-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/a49f7d43b743c46872cab66ff31a7471/transformed/REVLib-driver-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ba29cb9264109ae9ce10a3c2c4e7911e/transformed/wpiapi-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c8928631f160af67ff1c04c7385076b9/transformed/api-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/a03dc906d0672b10588d230a11e608fa/transformed/cci-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/2283db808f306fb0fa86a48712306830/transformed/wpiapi-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e8a2836d3180043d91ed4daed1612e4d/transformed/api-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/586d42abad815c0b0f34117fa93e20b0/transformed/cci-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e3e94461e07b8cb4cb778f297c838a6c/transformed/simTalonSRX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b3e54816efe38d5f889a3f9ea9330d16/transformed/simTalonFX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c80a43db59ea64498288dba883fe391e/transformed/simVictorSPX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/4af49850607ce2da9cfc6e6ab7851162/transformed/simPigeonIMU-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b9a9e42a33689df10221aaa788c97e85/transformed/simCANCoder-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/9e8a5a74c74c8235133be528f702b7f0/transformed/navx-cpp-4.0.442-headers/", - "/home/susan/.gradle/caches/transforms-3/6ec6f73e6581f4bfe9dad7033f036ad9/transformed/wpilibc-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/dd1fda39118691c5bf59ed1d17658648/transformed/ntcore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/bfd87a5cad0ec47eb318745d9dcf6c23/transformed/hal-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ee5d1035c4a3f1f67e8aec81b66e32d0/transformed/wpimath-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/41871ba548c354018cd047bceee44bab/transformed/wpiutil-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/1008ab75f241c8086800b8f7095e3eea/transformed/netcomm-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/f9d1d4885992a7a8b89bf4c594996390/transformed/chipobject-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/bdeaa1aa50eae116dc4e43a94f3a65f6/transformed/visa-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/4ffbd6d713a80732615525b599c34fbc/transformed/cscore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/509d89d78b44e4903a3f7e7858891526/transformed/opencv-cpp-4.5.2-1-headers/", - "/home/susan/SammyLogging/src/main/include/", - "/home/susan/.gradle/caches/transforms-3/a92563cc976ad00d7462a3d2072c1f71/transformed/wpilibNewCommands-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/ecc71bf6213e16f4067ad0977709a6ec/transformed/navx-cpp-4.0.442-sources/", - "/home/susan/.gradle/caches/transforms-3/608adad05b9f8ff2c94b10a2eb62ab21/transformed/wpilibc-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/15f7cf8ecf12f549e1556423b5d67945/transformed/ntcore-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/cd1cdce935056caf6ce35a26aca97513/transformed/hal-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/284b17bbeb445b40ad36c9fca15aa2d3/transformed/wpimath-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/d870fcde75bdcfa583e5698baaf46ba5/transformed/wpiutil-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/93ec4944ee5b9d314a0f3f2b9d73996b/transformed/cameraserver-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/bdbf41924a0dc282b242fccac16b8e11/transformed/cscore-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/75aacf4b83742d44efabec092f799296/transformed/opencv-cpp-4.5.2-1-sources/" - ], - "allLibSources": [ - "/home/susan/.gradle/caches/transforms-3/a92563cc976ad00d7462a3d2072c1f71/transformed/wpilibNewCommands-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/ecc71bf6213e16f4067ad0977709a6ec/transformed/navx-cpp-4.0.442-sources/", - "/home/susan/.gradle/caches/transforms-3/608adad05b9f8ff2c94b10a2eb62ab21/transformed/wpilibc-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/15f7cf8ecf12f549e1556423b5d67945/transformed/ntcore-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/cd1cdce935056caf6ce35a26aca97513/transformed/hal-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/284b17bbeb445b40ad36c9fca15aa2d3/transformed/wpimath-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/d870fcde75bdcfa583e5698baaf46ba5/transformed/wpiutil-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/93ec4944ee5b9d314a0f3f2b9d73996b/transformed/cameraserver-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/bdbf41924a0dc282b242fccac16b8e11/transformed/cscore-cpp-2022.3.1-sources/", - "/home/susan/.gradle/caches/transforms-3/75aacf4b83742d44efabec092f799296/transformed/opencv-cpp-4.5.2-1-sources/" - ], - "allLibHeaders": [ - "/home/susan/.gradle/caches/transforms-3/8c87b56b0ecd2defe52635c79f01e8e1/transformed/wpilibNewCommands-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/5e1f4de3a9a1449e6a39ad1a942acb9c/transformed/REVLib-cpp-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/a49f7d43b743c46872cab66ff31a7471/transformed/REVLib-driver-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ba29cb9264109ae9ce10a3c2c4e7911e/transformed/wpiapi-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c8928631f160af67ff1c04c7385076b9/transformed/api-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/a03dc906d0672b10588d230a11e608fa/transformed/cci-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/2283db808f306fb0fa86a48712306830/transformed/wpiapi-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e8a2836d3180043d91ed4daed1612e4d/transformed/api-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/586d42abad815c0b0f34117fa93e20b0/transformed/cci-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e3e94461e07b8cb4cb778f297c838a6c/transformed/simTalonSRX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b3e54816efe38d5f889a3f9ea9330d16/transformed/simTalonFX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c80a43db59ea64498288dba883fe391e/transformed/simVictorSPX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/4af49850607ce2da9cfc6e6ab7851162/transformed/simPigeonIMU-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b9a9e42a33689df10221aaa788c97e85/transformed/simCANCoder-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/9e8a5a74c74c8235133be528f702b7f0/transformed/navx-cpp-4.0.442-headers/", - "/home/susan/.gradle/caches/transforms-3/6ec6f73e6581f4bfe9dad7033f036ad9/transformed/wpilibc-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/dd1fda39118691c5bf59ed1d17658648/transformed/ntcore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/bfd87a5cad0ec47eb318745d9dcf6c23/transformed/hal-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ee5d1035c4a3f1f67e8aec81b66e32d0/transformed/wpimath-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/41871ba548c354018cd047bceee44bab/transformed/wpiutil-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/1008ab75f241c8086800b8f7095e3eea/transformed/netcomm-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/f9d1d4885992a7a8b89bf4c594996390/transformed/chipobject-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/bdeaa1aa50eae116dc4e43a94f3a65f6/transformed/visa-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/4ffbd6d713a80732615525b599c34fbc/transformed/cscore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/509d89d78b44e4903a3f7e7858891526/transformed/opencv-cpp-4.5.2-1-headers/", - "/home/susan/SammyLogging/src/main/include/" - ], - "binaries": [ - { - "componentName": "frcUserProgram", - "sourceSets": [ - { - "source": { - "srcDirs": [ - "/home/susan/SammyLogging/src/main/cpp/" - ], - "includes": [ - "**/*.cpp", - "**/*.cc" - ], - "excludes": [] - }, - "exportedHeaders": { - "srcDirs": [ - "/home/susan/SammyLogging/src/main/include/" - ], - "includes": [], - "excludes": [] - }, - "cpp": true, - "args": [ - "-Wall", - "-Wextra", - "-std=c++17", - "-Wformat=2", - "-pedantic", - "-Wno-psabi", - "-Wno-unused-parameter", - "-Wno-error=deprecated-declarations", - "-fPIC", - "-rdynamic", - "-pthread", - "-O2" - ], - "macros": [] - } - ], - "libHeaders": [ - "/home/susan/.gradle/caches/transforms-3/8c87b56b0ecd2defe52635c79f01e8e1/transformed/wpilibNewCommands-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/5e1f4de3a9a1449e6a39ad1a942acb9c/transformed/REVLib-cpp-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/a49f7d43b743c46872cab66ff31a7471/transformed/REVLib-driver-2022.1.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ba29cb9264109ae9ce10a3c2c4e7911e/transformed/wpiapi-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c8928631f160af67ff1c04c7385076b9/transformed/api-cpp-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/a03dc906d0672b10588d230a11e608fa/transformed/cci-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/2283db808f306fb0fa86a48712306830/transformed/wpiapi-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e8a2836d3180043d91ed4daed1612e4d/transformed/api-cpp-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/586d42abad815c0b0f34117fa93e20b0/transformed/cci-sim-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/e3e94461e07b8cb4cb778f297c838a6c/transformed/simTalonSRX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b3e54816efe38d5f889a3f9ea9330d16/transformed/simTalonFX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/c80a43db59ea64498288dba883fe391e/transformed/simVictorSPX-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/4af49850607ce2da9cfc6e6ab7851162/transformed/simPigeonIMU-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/b9a9e42a33689df10221aaa788c97e85/transformed/simCANCoder-5.20.2-headers/", - "/home/susan/.gradle/caches/transforms-3/9e8a5a74c74c8235133be528f702b7f0/transformed/navx-cpp-4.0.442-headers/", - "/home/susan/.gradle/caches/transforms-3/6ec6f73e6581f4bfe9dad7033f036ad9/transformed/wpilibc-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/dd1fda39118691c5bf59ed1d17658648/transformed/ntcore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/bfd87a5cad0ec47eb318745d9dcf6c23/transformed/hal-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/ee5d1035c4a3f1f67e8aec81b66e32d0/transformed/wpimath-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/41871ba548c354018cd047bceee44bab/transformed/wpiutil-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/1008ab75f241c8086800b8f7095e3eea/transformed/netcomm-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/f9d1d4885992a7a8b89bf4c594996390/transformed/chipobject-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/bdeaa1aa50eae116dc4e43a94f3a65f6/transformed/visa-2022.4.0-headers/", - "/home/susan/.gradle/caches/transforms-3/26cad6b34b0a67b7995d68f779eb83ea/transformed/cameraserver-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/4ffbd6d713a80732615525b599c34fbc/transformed/cscore-cpp-2022.3.1-headers/", - "/home/susan/.gradle/caches/transforms-3/509d89d78b44e4903a3f7e7858891526/transformed/opencv-cpp-4.5.2-1-headers/", - "/home/susan/SammyLogging/src/main/include/" - ], - "sharedLibrary": false, - "executable": true - } - ], - "sourceBinaries": [ - { - "source": { - "srcDirs": [ - "/home/susan/SammyLogging/src/main/cpp/" - ], - "includes": [ - "**/*.cpp", - "**/*.cc" - ], - "excludes": [] - }, - "componentName": "frcUserProgram", - "cpp": true, - "args": [ - "-Wall", - "-Wextra", - "-std=c++17", - "-Wformat=2", - "-pedantic", - "-Wno-psabi", - "-Wno-unused-parameter", - "-Wno-error=deprecated-declarations", - "-fPIC", - "-rdynamic", - "-pthread", - "-O2" - ], - "macros": [], - "sharedLibrary": false, - "executable": true - }, - { - "source": { - "srcDirs": [ - "/home/susan/SammyLogging/src/main/include/" - ], - "includes": [], - "excludes": [] - }, - "componentName": "frcUserProgram", - "cpp": true, - "args": [ - "-Wall", - "-Wextra", - "-std=c++17", - "-Wformat=2", - "-pedantic", - "-Wno-psabi", - "-Wno-unused-parameter", - "-Wno-error=deprecated-declarations", - "-fPIC", - "-rdynamic", - "-pthread", - "-O2" - ], - "macros": [], - "sharedLibrary": false, - "executable": true - } - ], - "nameBinaryMap": { - "frcUserProgram": 0 - } - } -] \ No newline at end of file diff --git a/pathplot.py b/pathplot.py new file mode 100644 index 0000000..a599adc --- /dev/null +++ b/pathplot.py @@ -0,0 +1,78 @@ +import numpy as np +from sys import stdin +import matplotlib.pyplot as plt +import math + +header = stdin.readline() +hdrs = [s.strip() for s in header.strip().split(",")] +assert hdrs == ["t", "x", "y", "theta", "xdot", "ydot", "thetadot"] + +t = [] +x = [] +y = [] +theta = [] +xdot = [] +ydot = [] +thetadot = [] +for line in stdin: + cells = [float(s.strip()) for s in line.strip().split(",")] + t.append(cells[0]) + x.append(cells[1]) + y.append(cells[2]) + theta.append(cells[3]) + xdot.append(cells[4]) + ydot.append(cells[5]) + thetadot.append(cells[6]) + + +# x vs y up dot, with theta arrows + +# x vs y vs theta vs time + +npxdot = np.array(xdot) +npydot = np.array(ydot) +nptdot = np.array(thetadot) + +ROBOT_R_METER = 0.520855 + +maxvel = np.sqrt(npxdot**2 + npydot**2) + np.abs(nptdot) * ROBOT_R_METER + + +plt.subplot(2, 1, 1) +plt.plot(x, y, linewidth=3) +for xx,yy,tt in zip(x, y, theta): + dx = 0.05 * math.cos(tt) + dy = 0.05 * math.sin(tt) + plt.arrow(xx, yy, dx, dy, width=0.01, color="red", head_width=0.02, head_length=0.02) +plt.xlabel('x') +plt.ylabel('y') + + + +plt.subplot(2, 4, 5) +plt.plot(t, x) +plt.xlabel('t') +plt.ylabel('x') + + +plt.subplot(2, 4, 6) +plt.plot(t, y) +plt.xlabel('t') +plt.ylabel('y') + + +plt.subplot(2, 4, 7) +plt.plot(t, theta) +plt.xlabel('t') +plt.ylabel('theta') + + + +plt.subplot(2, 4, 8) +plt.plot(t, maxvel) +plt.xlabel('t') +plt.ylabel('max wheel vel') + +# plt.tight_layout() + +plt.show() diff --git a/plotpath.sh b/plotpath.sh new file mode 100644 index 0000000..6659c2e --- /dev/null +++ b/plotpath.sh @@ -0,0 +1 @@ +./gradlew pathTestProgramDebugExecutable && ./build/exe/pathTestProgram/debug/pathTestProgram | python3 pathplot.py diff --git a/src/main/cpp/Odometry.cpp b/src/main/cpp/Odometry.cpp index d8669cc..012fd01 100644 --- a/src/main/cpp/Odometry.cpp +++ b/src/main/cpp/Odometry.cpp @@ -1,82 +1,74 @@ #include -//Constructor -Odometry::Odometry(){} - - -//Updates the Odometry Robot location -void -Odometry::updateOdometry(double BR_A, double BR_V, - double BL_A, double BL_V, - double FR_A, double FR_V, - double FL_A, double FL_V, - double theta){ - - double B_FL = sin(FL_A * PI/180) * FL_V; - double A_RL = sin(BL_A * PI/180) * BL_V; - double A_RR = sin(BR_A * PI/180) * BR_V; - double D_FL = cos(FL_A * PI/180) * FL_V; - double C_FR = cos(FR_A * PI/180) * FR_V; - double B_FR = sin(FR_A * PI/180) * FR_V; - double D_RL = cos(BL_A * PI/180) * BL_V; - //Potential Typo - double C_RR = cos(BR_A * PI/180) * BR_V; - - double A = (A_RR + A_RL) / 2; - double B = (B_FL + B_FR) / 2; - double C = (C_FR + C_RR) / 2; - double D = (D_FL + D_RL) / 2; - - double ROT1 = (B-A)/m_L; - double ROT2 = (C-D)/m_W; - double ROT = (ROT1 + ROT2) / 2; - - m_FWD = ((ROT * (m_L/2) + A) + (-ROT * (m_L/2) + B)) / 2; - m_STR = -((ROT * (m_W/2) + C) + (-ROT * (m_W/2) + D)) / 2; - - m_FWD_new = m_FWD * cos(theta * PI/180) + m_STR * sin(theta * PI/180); - m_STR_new = m_STR * cos(theta * PI/180) - m_FWD * sin(theta * PI/180); +// Constructor +Odometry::Odometry() {} + +// Updates the Odometry Robot location +void Odometry::updateOdometry(double br_angle_rad, double br_dist_m, + double bl_angle_rad, double bl_dist_m, + double fr_angle_rad, double fr_dist_m, + double fl_angle_rad, double fl_dist_m, + double theta_rad, double dt) { + if (!init) { + prev_bl = bl_dist_m; + prev_br = br_dist_m; + prev_fr = fr_dist_m; + prev_fl = fl_dist_m; + init = true; + return; + } + double delta_bl_dist_m = bl_dist_m - prev_bl; + prev_bl = bl_dist_m; + double delta_br_dist_m = br_dist_m - prev_br; + prev_br = br_dist_m; + double delta_fr_dist_m = fr_dist_m - prev_fr; + prev_fr = fr_dist_m; + double delta_fl_dist_m = fl_dist_m - prev_fl; + prev_fl = fl_dist_m; + + double br_y = sin(br_angle_rad) * delta_br_dist_m; + double br_x = cos(br_angle_rad) * delta_br_dist_m; + double bl_y = sin(bl_angle_rad) * delta_bl_dist_m; + double bl_x = cos(bl_angle_rad) * delta_bl_dist_m; + double fr_y = sin(fr_angle_rad) * delta_fr_dist_m; + double fr_x = cos(fr_angle_rad) * delta_fr_dist_m; + double fl_y = sin(fl_angle_rad) * delta_fl_dist_m; + double fl_x = cos(fl_angle_rad) * delta_fl_dist_m; + + double robot_local_dy = (fl_y + fr_y + bl_y + br_y) / 4.0; + double robot_local_dx = (fl_x + fr_x + bl_x + br_x) / 4.0; + + double global_dx = + robot_local_dx * cos(theta_rad) - robot_local_dy * sin(theta_rad); + double global_dy = + robot_local_dx * sin(theta_rad) + robot_local_dy * cos(theta_rad); // frc::SmartDashboard::PutNumber("m_FWD", m_FWD_new); // frc::SmartDashboard::PutNumber("m_STR", m_STR_new); - - //From Velocity to Position - m_prevtimeStep = m_timeStep; - m_timeStep += timeInterval; - m_x = m_x + m_FWD_new * (m_timeStep - m_prevtimeStep) * 58.04; - m_y = m_y + m_STR_new * (m_timeStep - m_prevtimeStep) * 58.04; -} + m_xdot = global_dx / dt; + m_ydot = global_dy / dt; -//Return X position -double -Odometry::getX(){ - return m_x; + // From Velocity to Position + m_x += global_dx; + m_y += global_dy; } +// Return X position +double Odometry::getX() { return m_x; } -//Return Y position -double -Odometry::getY(){ - return m_y; -} - +// Return Y position +double Odometry::getY() { return m_y; } -double -Odometry::getYSpeed(){ - return m_FWD_new; -} +double Odometry::getYSpeed() { return m_ydot; } +double Odometry::getXSpeed() { return m_xdot; } -double -Odometry::getXSpeed(){ - return m_STR_new; -} - - -//Reset Odometry -void -Odometry::Reset(){ +// Reset Odometry, without resetting seen encoder values. +// To renormalize to seen encoder values, call updateOdometry, +// then Reset. +void Odometry::Reset() { m_x = 0; m_y = 0; - m_timeStep = 0; -} \ No newline at end of file + m_xdot = 0; + m_ydot = 0; +} diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 9a2c404..72734f7 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -1,221 +1,209 @@ #include "Robot.h" -#include -#include - -void -Robot::RobotInit() { - m_chooser.SetDefaultOption("Blue", blueAlliance); - m_chooser.AddOption("RED", redAlliance); - frc::SmartDashboard::PutData("Alliance Color", &m_chooser); - - camera = frc::CameraServer::GetInstance()->StartAutomaticCapture(); - - try{ - navx = new AHRS(frc::SPI::Port::kMXP); - } catch(const std::exception& e){ - std::cout << e.what() < +#include -void -Robot::RobotPeriodic() { -} +void Robot::RobotInit() { + m_chooser.SetDefaultOption("Blue", blueAlliance); + m_chooser.AddOption("RED", redAlliance); + frc::SmartDashboard::PutData("Alliance Color", &m_chooser); + frc::CameraServer::StartAutomaticCapture(); -void -Robot::AutonomousInit() { - m_auto.ResetAuto(); - m_time = 0; - m_swerve.ResetOdometry(); - m_swerve.GenerateTrajectory_1(); - m_swerve.Initialize(); - m_intake.Deploy(); - m_shooter.setState(Shooter::State::IDLE); - m_intake.setState(Intake::State::IDLE); - m_shooter.Zero(); - PDH.ClearStickyFaults(); - navx->Reset(); - - //This might not work?? - m_climber.Initialize(); + try { + navx = new AHRS(frc::SPI::Port::kMXP); + } catch (const std::exception &e) { + std::cout << e.what() << std::endl; + } + m_swerve.InjectNavx(navx); + m_climbing = false; } - -void -Robot::AutonomousPeriodic() { - m_time += m_timeStep; - m_auto.Periodic(m_time); - //frc::SmartDashboard::PutNumber("Y", m_swerve.GetYPosition()); - //frc::SmartDashboard::PutNumber("X", m_swerve.GetXPostion()); - - switch(m_auto.getState()){ - case AutoMode::State::IDLE: - m_intake.setState(Intake::IDLE); - m_shooter.setState(Shooter::IDLE); - m_swerve.Drive(0, 0, 0, 0, true); - break; - case AutoMode::State::DRIVEnINTAKE: - m_intake.setState(Intake::State::RUN); - m_shooter.setState(Shooter::State::LOAD); - m_swerve.TrajectoryFollow(navx->GetYaw(), m_auto.getWaypointIndex()); - break; - case AutoMode::State::SHOOT: - m_shooter.setState(Shooter::State::SHOOT); - break; - case AutoMode::State::DRIVE: - m_swerve.TrajectoryFollow(navx->GetYaw(), m_auto.getWaypointIndex()); - break; - case AutoMode::State::INTAKE: - // m_intake.setState(Intake::State::RUN); - // m_shooter.setState(Shooter::State::LOAD); - break; - default: - break; - } - - m_intake.Periodic(); - m_shooter.Periodic(m_swerve.GetXPosition(), m_swerve.GetYPosition()); +void Robot::RobotPeriodic() { + m_swerve.UpdateOdometry(m_timeStep); + m_intake.Periodic(); + m_shooter.Periodic(m_swerve.GetXPosition(), m_swerve.GetYPosition()); } - -void -Robot::TeleopInit() { - //Put this in robotinit? - if(m_chooser.GetSelected() == blueAlliance){ - m_shooter.setColor(true); - } else { - m_shooter.setColor(false); - } - - m_time = 0; - navx->Reset(); - - m_shooter.setState(Shooter::State::IDLE); - m_intake.setState(Intake::State::IDLE); - m_swerve.Initialize(); - - //REMOVE THIS WHEN AT COMPETITION! - m_shooter.Zero(); - - PDH.ClearStickyFaults(); - m_intake.Deploy(); - m_shooter.Periodic(m_swerve.GetXPosition(), m_swerve.GetYPosition()); - m_intake.Periodic(); - m_climber.Initialize(); +void Robot::AutonomousInit() { + m_auto.ResetAuto(); + m_time = 0; + m_swerve.ResetOdometry(); + navx->Reset(); + m_swerve.GenerateTrajectory_1(); + m_swerve.Initialize(); + m_intake.Deploy(); + m_shooter.setState(Shooter::State::IDLE); + m_intake.setState(Intake::State::IDLE); + m_shooter.Zero(); + PDH.ClearStickyFaults(); + + // This might not work?? + m_climber.Initialize(); + + // // the "new" automodes would be used like + // std::string choice = getSendableChooserValue(); + // if (choice == "TwoBall") { + // m_automode = + // std::make_unique(&m_swerve, &m_shooter, &m_intake); + // } else if (choice == "OtherAuto") { + // m_automode = + // std::make_unique(&m_swerve, &m_shooter, &m_intake); + // } else { + // m_automode = + // std::make_unique(&m_swerve, &m_shooter, &m_intake); + // } + // m_automode->start() } +void Robot::AutonomousPeriodic() { + m_time += m_timeStep; + m_auto.Periodic(m_time); -void -Robot::TeleopPeriodic() { - m_time += m_timeStep; - double x1, y1, x2; - x1 = l_joy.GetRawAxis(0) * 0.7; - y1 = l_joy.GetRawAxis(1); - x2 = r_joy.GetRawAxis(0); - x1 = abs(x1) < 0.05 ? 0.0: x1; - y1 = abs(y1) < 0.05 ? 0.0: y1; - x2 = abs(x2) < 0.05 ? 0.0: x2; - - m_swerve.Drive(-x1*0.6, -y1, -x2, navx->GetYaw(), true); - - //Climbing - if(m_climbing){ - if(xbox.GetRawButtonPressed(2)){ - m_climber.ExtendsecondStage(); - } - else if(xbox.GetRawButtonPressed(3)){ - m_climber.ExtendfirstStage(); - - } - else if(xbox.GetRightBumper()){ - out = 0; - m_climber.armExtension(out); - } - else if(abs(xbox.GetRawAxis(1)) > 0.1){ - out = xbox.GetRawAxis(1); - if(abs(out) < 0.3){ - out = 0; - } - m_climber.armExtension(out); - } - } else { - //Teleop Operation - //Intake - if(r_joy.GetTrigger()){ - m_intake.setState(Intake::State::RUN); - m_shooter.setState(Shooter::State::LOAD); - } - //Shoot - else if(l_joy.GetTrigger()){ - m_shooter.setState(Shooter::State::SHOOT); - } - //Manual turret movement - else if(abs(xbox.GetRawAxis(4)) > 0.2 ){ - m_shooter.Manual(xbox.GetRawAxis(4)); - m_shooter.setState(Shooter::State::MANUAL); - } - //back button will enable climb - else if(xbox.GetBackButtonPressed()){ - m_climbing = true; - } - //button A will reset robot yaw or outtake - else if(xbox.GetStartButtonPressed()){ - navx->Reset(); - // m_shooter.setPID(); - // m_shooter.Calibrate(); - } - else if(xbox.GetRawButton(1)){ - m_intake.setState(Intake::State::UNJAM); - m_shooter.setState(Shooter::BadIdea); - } - // else if(xbox.GetRawButton(4)){ - // m_shooter.LowShot(); - // } - // else if(l_joy.GetPOV() != -1){ - //std::cout << "Peek" << std::endl; - // m_shooter.peekTurret(); + // // The "new" automodes would be used like: + // if (m_automode) { + // m_automode->tick(time); // } - else { - m_shooter.setState(Shooter::State::IDLE); - m_intake.setState(Intake::State::IDLE); - } - m_intake.Periodic(); - m_shooter.Periodic(m_swerve.GetXPosition(), m_swerve.GetYPosition()); - } - //frc::SmartDashboard::PutNumber("Yaw", navx->GetYaw()); - //frc::SmartDashboard::PutNumber("POV", l_joy.GetPOV()); + // frc::SmartDashboard::PutNumber("Y", m_swerve.GetYPosition()); + // frc::SmartDashboard::PutNumber("X", m_swerve.GetXPosition()); + + switch (m_auto.getState()) { + case AutoMode::State::IDLE: + m_intake.setState(Intake::IDLE); + m_shooter.setState(Shooter::IDLE); + m_swerve.Drive(0, 0, 0, 0, true); + break; + case AutoMode::State::DRIVEnINTAKE: + m_intake.setState(Intake::State::RUN); + m_shooter.setState(Shooter::State::LOAD); + m_swerve.TrajectoryFollow(navx->GetYaw(), + m_auto.getWaypointIndex()); + break; + case AutoMode::State::SHOOT: + m_shooter.setState(Shooter::State::SHOOT); + break; + case AutoMode::State::DRIVE: + m_swerve.TrajectoryFollow(navx->GetYaw(), + m_auto.getWaypointIndex()); + break; + case AutoMode::State::INTAKE: + // m_intake.setState(Intake::State::RUN); + // m_shooter.setState(Shooter::State::LOAD); + break; + default: + break; + } } +void Robot::TeleopInit() { + // Put this in robotinit? + if (m_chooser.GetSelected() == blueAlliance) { + m_shooter.setColor(true); + } else { + m_shooter.setColor(false); + } -void -Robot::TestInit() { -} + m_time = 0; + navx->Reset(); + m_shooter.setState(Shooter::State::IDLE); + m_intake.setState(Intake::State::IDLE); + m_swerve.Initialize(); -void -Robot::TestPeriodic() { -} + // REMOVE THIS WHEN AT COMPETITION! + m_shooter.Zero(); + PDH.ClearStickyFaults(); + m_intake.Deploy(); + m_climber.Initialize(); +} -void -Robot::DisabledInit() { - m_climber.enableBrake(); - m_climber.whenDisabled(); +void Robot::TeleopPeriodic() { + m_time += m_timeStep; + double x1, y1, x2; + x1 = l_joy.GetRawAxis(0) * 0.7; + y1 = l_joy.GetRawAxis(1); + x2 = r_joy.GetRawAxis(0); + x1 = abs(x1) < 0.05 ? 0.0 : x1; + y1 = abs(y1) < 0.05 ? 0.0 : y1; + x2 = abs(x2) < 0.05 ? 0.0 : x2; + + m_swerve.Drive(-x1 * 0.6, -y1, -x2, navx->GetYaw(), true); + + // Climbing + if (m_climbing) { + if (xbox.GetRawButtonPressed(2)) { + m_climber.ExtendsecondStage(); + } else if (xbox.GetRawButtonPressed(3)) { + m_climber.ExtendfirstStage(); + } else if (xbox.GetRightBumper()) { + out = 0; + m_climber.armExtension(out); + } else if (abs(xbox.GetRawAxis(1)) > 0.1) { + out = xbox.GetRawAxis(1); + if (abs(out) < 0.3) { + out = 0; + } + m_climber.armExtension(out); + } + } else { + // Teleop Operation + // Intake + if (r_joy.GetTrigger()) { + m_intake.setState(Intake::State::RUN); + m_shooter.setState(Shooter::State::LOAD); + } + // Shoot + else if (l_joy.GetTrigger()) { + m_shooter.setState(Shooter::State::SHOOT); + } + // Manual turret movement + else if (abs(xbox.GetRawAxis(4)) > 0.2) { + m_shooter.Manual(xbox.GetRawAxis(4)); + m_shooter.setState(Shooter::State::MANUAL); + } + // back button will enable climb + else if (xbox.GetBackButtonPressed()) { + m_climbing = true; + } + // button A will reset robot yaw or outtake + else if (xbox.GetStartButtonPressed()) { + navx->Reset(); + // m_shooter.setPID(); + // m_shooter.Calibrate(); + } else if (xbox.GetRawButton(1)) { + m_intake.setState(Intake::State::UNJAM); + m_shooter.setState(Shooter::BadIdea); + } + // else if(xbox.GetRawButton(4)){ + // m_shooter.LowShot(); + // } + // else if(l_joy.GetPOV() != -1){ + // std::cout << "Peek" << std::endl; + // m_shooter.peekTurret(); + // } + else { + m_shooter.setState(Shooter::State::IDLE); + m_intake.setState(Intake::State::IDLE); + } + } + // frc::SmartDashboard::PutNumber("Yaw", navx->GetYaw()); + // frc::SmartDashboard::PutNumber("POV", l_joy.GetPOV()); } +void Robot::TestInit() {} -void -Robot::DisabledPeriodic() {} +void Robot::TestPeriodic() {} +void Robot::DisabledInit() { + m_climber.enableBrake(); + m_climber.whenDisabled(); +} +void Robot::DisabledPeriodic() {} #ifndef RUNNING_FRC_TESTS -int main() { - return frc::StartRobot(); -} -#endif \ No newline at end of file +int main() { return frc::StartRobot(); } +#endif diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp index 560f72f..18f313c 100644 --- a/src/main/cpp/Shooter.cpp +++ b/src/main/cpp/Shooter.cpp @@ -1,8 +1,9 @@ +#include #include -#include +#include -Shooter::Shooter(){ +Shooter::Shooter() { m_flywheelMaster.SetNeutralMode(NeutralMode::Coast); m_flywheelSlave.SetNeutralMode(NeutralMode::Coast); m_hood.SetNeutralMode(NeutralMode::Coast); @@ -12,10 +13,13 @@ Shooter::Shooter(){ m_flywheelMaster.SetSafetyEnabled(false); m_flywheelSlave.SetSafetyEnabled(false); m_turret.SetSafetyEnabled(false); - - m_flywheelMaster.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor); - m_flywheelSlave.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor); - m_kicker.SetStatusFramePeriod(StatusFrameEnhanced::Status_12_Feedback1, 100); + + m_flywheelMaster.ConfigSelectedFeedbackSensor( + FeedbackDevice::IntegratedSensor); + m_flywheelSlave.ConfigSelectedFeedbackSensor( + FeedbackDevice::IntegratedSensor); + m_kicker.SetStatusFramePeriod(StatusFrameEnhanced::Status_12_Feedback1, + 100); m_flywheelMaster.Config_kF(0, ShooterConstants::flywheelF); m_flywheelMaster.Config_kP(0, ShooterConstants::flywheelP); @@ -41,7 +45,7 @@ Shooter::Shooter(){ dataMap[-20.0] = {5500, 18200}; dataMap[-19.2] = {5500, 18000}; dataMap[-18.0] = {5400, 17400}; - dataMap[-16.7] = {5400, 17000}; + dataMap[-16.7] = {5400, 17000}; dataMap[-15.4] = {4900, 15700}; dataMap[-14.5] = {4600, 15500}; dataMap[-12.6] = {4500, 14500}; @@ -66,14 +70,11 @@ Shooter::Shooter(){ m_colorMatcher.AddColorMatch(defualtColor); } +Shooter::~Shooter() {} -Shooter::~Shooter(){} - - -//Periodic Function -void -Shooter::Periodic(double robotX, double robotY){ - switch(m_state){ +// Periodic Function +void Shooter::Periodic(double robotX, double robotY) { + switch (m_state) { case State::SHOOT: m_robotX = robotX; m_robotY = robotY; @@ -107,53 +108,55 @@ Shooter::Periodic(double robotX, double robotY){ } m_channel.Periodic(); - //frc::SmartDashboard::PutNumber("Red", m_colorSensor.GetColor().red); - //frc::SmartDashboard::PutNumber("blue", m_colorSensor.GetColor().blue); - //frc::SmartDashboard::PutNumber("green", m_colorSensor.GetColor().green); - + // frc::SmartDashboard::PutNumber("Red", m_colorSensor.GetColor().red); + // frc::SmartDashboard::PutNumber("blue", m_colorSensor.GetColor().blue); + // frc::SmartDashboard::PutNumber("green", m_colorSensor.GetColor().green); } - -//Aim Function for Turret -void -Shooter::Aim(){ +// Aim Function for Turret +void Shooter::Aim() { m_limelight->setLEDMode("ON"); - if(m_colorMatcher.MatchClosestColor(m_colorSensor.GetColor(), confidence) == ballColor){ + if (m_colorMatcher.MatchClosestColor(m_colorSensor.GetColor(), + confidence) == ballColor) { m_speed = 8000; m_angle = 100; m_hood.Set(ControlMode::Position, m_angle); m_flywheelMaster.Set(ControlMode::Velocity, m_speed); m_flywheelSlave.Set(ControlMode::Velocity, -m_speed); m_channel.Run(); - + Load(); return; } - //Check if the limelight sees the target + // Check if the limelight sees the target double point = m_limelight->getYOff(); - //If point is not found - if(point == 10000.0 && m_OdometryErrorRate < 10){ - //Law of cosines - //CalcAng = angle from hub to last recorded location to current location - double calcAng = PI+(-m_lastAngle*PI/180)-atan2((m_robotY-m_lastY,m_robotX-m_lastX); - double changeDist = pow(m_robotX-m_lastX,2)+pow(m_robotY-m_lastY,2); - point = sqrt(pow(m_lastDist,2)+changeDist-(2*m_lastDist*sqrt(changeDist)*cos(calcAng))); + // If point is not found + if (point == 10000.0 && m_OdometryErrorRate < 10) { + // Law of cosines + // CalcAng = angle from hub to last recorded location to current + // location + double calcAng = PI + (-m_lastAngle * PI / 180) - + atan2(m_robotY - m_lastY, m_robotX - m_lastX); + double changeDist = + pow(m_robotX - m_lastX, 2) + pow(m_robotY - m_lastY, 2); + point = sqrt(pow(m_lastDistance, 2) + changeDist - + (2 * m_lastDistance * sqrt(changeDist) * cos(calcAng))); } - //Calculate + // Calculate auto data = dataMap.find(point); - if(data != dataMap.end()){ + if (data != dataMap.end()) { m_angle = data->second.first; m_speed = data->second.second; - //Sets last known X, Y, and distance + // Sets last known X, Y, and distance m_lastX = m_robotX; m_lastY = m_robotY; m_lastDistance = point; } else { double point1, point2; - if(withinRange(dataPoints, point, point1, point2)){ + if (withinRange(dataPoints, point, point1, point2)) { auto data1 = dataMap[point1]; auto data2 = dataMap[point2]; double angle1, speed1; @@ -163,109 +166,107 @@ Shooter::Aim(){ angle2 = data2.first; speed2 = data2.second; - m_angle = (angle1 + angle2)/2 * angle_scale_factor; - m_speed = (speed1 + speed2)/2 * speed_scale_factor; - //frc::SmartDashboard::PutNumber("velocity", m_speed); - //frc::SmartDashboard::PutNumber("position", m_angle); + m_angle = (angle1 + angle2) / 2 * angle_scale_factor; + m_speed = (speed1 + speed2) / 2 * speed_scale_factor; + // frc::SmartDashboard::PutNumber("velocity", m_speed); + // frc::SmartDashboard::PutNumber("position", m_angle); - //Sets last known X, Y, and distance + // Sets last known X, Y, and distance m_lastX = m_robotX; m_lastY = m_robotY; m_lastDistance = point; - }else{ + } else { m_angle = 0; m_speed = 0; } } // Set Turret movement - double x_off = m_limelight->getXOff()+3.85; - if(x_off == 10000.0 && m_OdometryErrorRate < 10){ - //tan of distance between hub and robot - double hubX = m_lastDistance*cos(m_lastAngle/PI*180) - double hubY = m_lastDistance*sin(m_lastAngle/PI*180) - x_off = (tan2(hubY- m_lastY + m_robotY, hubX - m_lastX + m_robotX))/PI*180 - m_turrPos; + double x_off = m_limelight->getXOff() + 3.85; + if (x_off == 10000.0 && m_OdometryErrorRate < 10) { + // tan of distance between hub and robot + double hubX = m_lastDistance * cos(m_lastAngle / PI * 180); + double hubY = m_lastDistance * sin(m_lastAngle / PI * 180); + x_off = (atan2(hubY - m_lastY + m_robotY, hubX - m_lastX + m_robotX)) / + PI * 180 - + m_turrPos; } - //Record Last Angle - else{ + // Record Last Angle + else { m_lastAngle = x_off + m_turrPos; } double output = -m_turretController.Calculate(x_off); - output = output > 0 && output > 0.4 ? 0.4: output; - output = output < 0 && output < -0.4 ? -0.4: output; - if(m_turret.GetSelectedSensorPosition() > ShooterConstants::turretMax && - output > 0){ - m_turret.Set(ControlMode::PercentOutput, 0.0); + output = output > 0 && output > 0.4 ? 0.4 : output; + output = output < 0 && output < -0.4 ? -0.4 : output; + if (m_turret.GetSelectedSensorPosition() > ShooterConstants::turretMax && + output > 0) { + m_turret.Set(ControlMode::PercentOutput, 0.0); return; } - if(m_turret.GetSelectedSensorPosition() < ShooterConstants::turretMin && - output < 0){ + if (m_turret.GetSelectedSensorPosition() < ShooterConstants::turretMin && + output < 0) { m_turret.Set(ControlMode::PercentOutput, 0.0); return; - } - else { + } else { m_turret.Set(output); } // Set Flywheel Velocity m_flywheelMaster.Set(ControlMode::Velocity, m_speed); m_flywheelSlave.Set(ControlMode::Velocity, -m_speed); - frc::SmartDashboard::PutNumber("flywheel speed", m_flywheelMaster.GetSelectedSensorVelocity()); - frc::SmartDashboard::PutNumber("hood angle", m_hood.GetSelectedSensorPosition()); - + frc::SmartDashboard::PutNumber( + "flywheel speed", m_flywheelMaster.GetSelectedSensorVelocity()); + frc::SmartDashboard::PutNumber("hood angle", + m_hood.GetSelectedSensorPosition()); + // Set Hood Position m_hood.Set(ControlMode::Position, m_angle); - if(m_hood.GetSupplyCurrent() >= ShooterConstants::zeroingcurrent){ + if (m_hood.GetSupplyCurrent() >= ShooterConstants::zeroingcurrent) { m_hood.Set(ControlMode::PercentOutput, 0.0); } - //frc::SmartDashboard::PutNumber("yOff", m_limelight->getYOff()); - //frc::SmartDashboard::PutNumber("xOff", m_limelight->getXOff()); + // frc::SmartDashboard::PutNumber("yOff", m_limelight->getYOff()); + // frc::SmartDashboard::PutNumber("xOff", m_limelight->getXOff()); } - -//Interpolate shooter point values -bool -Shooter::withinRange(std::vector& array, double p, double& p1, double& p2){ - int left =0; - int right = array.size() -1; - if(p < array[left] || p > array[right]){ +// Interpolate shooter point values +bool Shooter::withinRange(std::vector& array, double p, double& p1, + double& p2) { + int left = 0; + int right = array.size() - 1; + if (p < array[left] || p > array[right]) { return false; } - int mid = (left + right)/2; - while(left < right){ - if(array[mid] <= p && p <= array[mid+1]){ + int mid = (left + right) / 2; + while (left < right) { + if (array[mid] <= p && p <= array[mid + 1]) { p1 = array[mid]; - p2 = array[mid+1]; + p2 = array[mid + 1]; return true; - } else if(array[mid] <= p){ + } else if (array[mid] <= p) { left = mid; - } else if(array[mid] >= p){ + } else if (array[mid] >= p) { right = mid; } - mid = (left + right)/2; + mid = (left + right) / 2; } return false; } - -//Shoot Function -void -Shooter::Shoot(){ - if(Aimed()){ +// Shoot Function +void Shooter::Shoot() { + if (Aimed()) { Load(); - // m_channel.setState(Channel::State::RUN); - // Load(); - // } else { - // m_channel.setState(Channel::State::IDLE); - // } + // m_channel.setState(Channel::State::RUN); + // Load(); + // } else { + // m_channel.setState(Channel::State::IDLE); + // } } } - -//Zero Function -//fix -void -Shooter::Zero(){ +// Zero Function +// fix +void Shooter::Zero() { m_turret.SetSelectedSensorPosition(-31500); m_turretZero = true; m_hood.SetSelectedSensorPosition(0); @@ -280,19 +281,16 @@ Shooter::Zero(){ // m_turret.Set(ControlMode::PercentOutput, 0.20); // } // if(!m_hoodZero){ - // m_hood.Set(ControlMode::PercentOutput, -0.30); + // m_hood.Set(ControlMode::PercentOutput, -0.30); // } } - -//Load Function -void -Shooter::Load(){ - if(m_state == State::SHOOT){ +// Load Function +void Shooter::Load() { + if (m_state == State::SHOOT) { m_kicker.Set(ControlMode::PercentOutput, 0.2); - } - else if(m_state == State::LOAD){ - if(!m_photogate.Get()){ + } else if (m_state == State::LOAD) { + if (!m_photogate.Get()) { m_kicker.Set(ControlMode::PercentOutput, 0.0); m_channel.setState(Channel::State::IDLE); } else { @@ -302,35 +300,28 @@ Shooter::Load(){ } } +// set the State of the shooter +void Shooter::setState(State newState) { m_state = newState; } -//set the State of the shooter -void -Shooter::setState(State newState){ - m_state = newState; -} - - -//Check if the turret is aimed -bool -Shooter::Aimed(){ - bool flywheelReady = abs(m_flywheelMaster.GetSelectedSensorVelocity() - m_speed) < 500; +// Check if the turret is aimed +bool Shooter::Aimed() { + bool flywheelReady = + abs(m_flywheelMaster.GetSelectedSensorVelocity() - m_speed) < 500; bool hoodReady = abs(m_hood.GetSelectedSensorPosition() - m_angle) < 250; - bool turretReady = abs(m_limelight->getXOff()+3.85) < 3; + bool turretReady = abs(m_limelight->getXOff() + 3.85) < 3; frc::SmartDashboard::PutBoolean("flywheelReady", flywheelReady); frc::SmartDashboard::PutBoolean("hoodReady", hoodReady); frc::SmartDashboard::PutBoolean("turretReady", turretReady); - if( flywheelReady && hoodReady && turretReady){ + if (flywheelReady && hoodReady && turretReady) { return true; } else { return false; } } - -//Calibrate function -void -Shooter::Calibrate(){ +// Calibrate function +void Shooter::Calibrate() { // m_speed = frc::SmartDashboard::GetNumber("speed", 0.0); // m_angle = frc::SmartDashboard::GetNumber("angle", 0.0); // frc::SmartDashboard::PutNumber("speed", m_speed); @@ -339,10 +330,8 @@ Shooter::Calibrate(){ frc::SmartDashboard::PutNumber("turr_pos", m_turrPos); } - -//Set PID values -void -Shooter::setPID(){ +// Set PID values +void Shooter::setPID() { double P = frc::SmartDashboard::GetNumber("P", 0.0); frc::SmartDashboard::PutNumber("P", P); double I = frc::SmartDashboard::GetNumber("I", 0.0); @@ -363,20 +352,17 @@ Shooter::setPID(){ // m_flywheelSlave.Config_kP(0, P); // m_flywheelSlave.Config_kI(0, I); // m_flywheelSlave.Config_kD(0, D); - - //m_turretController.SetPID(P, I, D); - + + // m_turretController.SetPID(P, I, D); + // m_hood.Config_kP(0, P); // m_hood.Config_kI(0, I); // m_hood.Config_kD(0, D); // m_hood.Config_kF(0, F); - } - -//Stop All shooter movements -void -Shooter::Stop(){ +// Stop All shooter movements +void Shooter::Stop() { m_turret.Set(0); m_flywheelSlave.Set(0); m_flywheelMaster.Set(0); @@ -386,33 +372,29 @@ Shooter::Stop(){ m_channel.setState(Channel::State::IDLE); } - -//Manual turret movement -void -Shooter::Manual(double turret_rot){ - if(abs(turret_rot) <= 0.05){ +// Manual turret movement +void Shooter::Manual(double turret_rot) { + if (abs(turret_rot) <= 0.05) { turret_rot = 0; } - if(turret_rot > 0 && m_turret.GetSelectedSensorPosition() > ShooterConstants::turretMax){ + if (turret_rot > 0 && + m_turret.GetSelectedSensorPosition() > ShooterConstants::turretMax) { turret_rot = 0; } - if(turret_rot < 0 && m_turret.GetSelectedSensorPosition() < ShooterConstants::turretMin){ + if (turret_rot < 0 && + m_turret.GetSelectedSensorPosition() < ShooterConstants::turretMin) { turret_rot = 0; } - //frc::SmartDashboard::PutNumber("manual", turret_rot * 0.2); - m_turret.Set(ControlMode::PercentOutput, turret_rot*0.2); + // frc::SmartDashboard::PutNumber("manual", turret_rot * 0.2); + m_turret.Set(ControlMode::PercentOutput, turret_rot * 0.2); } - -//Convert unit per 100 ms to rpm -double -Shooter::convertToRPM(double ticks){ - return ticks/ShooterConstants::TalonFXunitsperrot*600; +// Convert unit per 100 ms to rpm +double Shooter::convertToRPM(double ticks) { + return ticks / ShooterConstants::TalonFXunitsperrot * 600; } - -void -Shooter::DisableMotors(){ +void Shooter::DisableMotors() { m_kicker.Disable(); m_hood.Disable(); m_flywheelMaster.Disable(); @@ -420,23 +402,19 @@ Shooter::DisableMotors(){ m_channel.DisableMotor(); } - -void -Shooter::setColor(bool isblue){ +void Shooter::setColor(bool isblue) { m_blue = isblue; - ballColor = m_blue? redBall : blueBall; + ballColor = m_blue ? redBall : blueBall; } - -void -Shooter::LowShot(){ +void Shooter::LowShot() { m_hood.Set(ControlMode::Position, 1500); m_flywheelMaster.Set(ControlMode::PercentOutput, 0.5); m_flywheelSlave.Set(ControlMode::PercentOutput, -0.5); } - -void -Shooter::peekTurret(){ - m_turret.Set(std::clamp(m_turretPos.Calculate(m_turret.GetSelectedSensorPosition(), m_turrPos), -0.5, 0.5)); -} \ No newline at end of file +void Shooter::peekTurret() { + m_turret.Set(std::clamp( + m_turretPos.Calculate(m_turret.GetSelectedSensorPosition(), m_turrPos), + -0.5, 0.5)); +} diff --git a/src/main/cpp/SwerveDrive.cpp b/src/main/cpp/SwerveDrive.cpp index 11bad2f..fe10d65 100644 --- a/src/main/cpp/SwerveDrive.cpp +++ b/src/main/cpp/SwerveDrive.cpp @@ -1,40 +1,55 @@ #include "SwerveDrive.h" -#include -//Constructor -SwerveDrive::SwerveDrive(){} +#include +#include +// Constructor +SwerveDrive::SwerveDrive() {} -//Drive function contains Swerve Inverse Kinematics -void -SwerveDrive::Drive(double x1, double y1, double x2, double rot, bool fieldOrient){ - double r = sqrt(m_L*m_L + m_W*m_W); +// Drive function contains Swerve Inverse Kinematics +void SwerveDrive::Drive(double x1, double y1, double x2, double rot, + bool fieldOrient) { + double r = sqrt(m_L * m_L + m_W * m_W); y1 *= -1; x1 *= -1; x2 *= -1; - rot *= M_PI/180; + rot *= M_PI / 180; - if(fieldOrient){ - m_temp = y1*cos(rot) + x1*sin(rot); - x1 = -y1*sin(rot) + x1*cos(rot); + if (fieldOrient) { + m_temp = y1 * cos(rot) + x1 * sin(rot); + x1 = -y1 * sin(rot) + x1 * cos(rot); y1 = m_temp; } - double a = x1 - x2 * (m_L/r); - double b = x1 + x2 * (m_L/r); - double c = y1 - x2 * (m_W/r); - double d = y1 + x2 * (m_W/r); - - double backRightSpeed = sqrt((a*a) + (d*d)); - double backLeftSpeed = sqrt((a*a) + (c*c)); - double frontRightSpeed = sqrt((b*b) + (d*d)); - double frontLeftSpeed = sqrt((b*b) + (c*c)); + double a = x1 - x2 * (m_L / r); + double b = x1 + x2 * (m_L / r); + double c = y1 - x2 * (m_W / r); + double d = y1 + x2 * (m_W / r); + + double backRightSpeed = sqrt((a * a) + (d * d)); + double backLeftSpeed = sqrt((a * a) + (c * c)); + double frontRightSpeed = sqrt((b * b) + (d * d)); + double frontLeftSpeed = sqrt((b * b) + (c * c)); + + // If the commanded wheel speeds are not achievable, maintain their ratio. + // If we didn't do this, some motors would saturate, breaking the manuevre. + double max = std::max(std::max(backRightSpeed, backLeftSpeed), + std::max(frontRightSpeed, frontLeftSpeed)); + if (max > 1.0) { + backRightSpeed /= max; + backLeftSpeed /= max; + frontRightSpeed /= max; + frontLeftSpeed /= max; + // Remove this if it fires often: + std::cout << "Swerve drive cmd renormalized, prior max was " << max + << std::endl; + } - double backRightAngle = atan2(a, d) * 180/ M_PI; - double backLeftAngle = atan2(a, c) *180/ M_PI; - double frontRightAngle = atan2(b, d) *180/ M_PI; - double frontLeftAngle = atan2(b, c) *180/ M_PI; + double backRightAngle = atan2(a, d) * 180 / M_PI; + double backLeftAngle = atan2(a, c) * 180 / M_PI; + double frontRightAngle = atan2(b, d) * 180 / M_PI; + double frontLeftAngle = atan2(b, c) * 180 / M_PI; - if(x2 == 0 && x1 == 0 && y1 == 0){ + if (x2 == 0 && x1 == 0 && y1 == 0) { m_backRight.Stop(); m_backLeft.Stop(); m_frontRight.Stop(); @@ -42,81 +57,122 @@ SwerveDrive::Drive(double x1, double y1, double x2, double rot, bool fieldOrient return; } - m_backRight.drive(-backRightSpeed, backRightAngle, - DriveConstants::BROFF); - m_backLeft.drive(backLeftSpeed, backLeftAngle, - DriveConstants::BLOFF); + m_backRight.drive(-backRightSpeed, backRightAngle, DriveConstants::BROFF); + m_backLeft.drive(backLeftSpeed, backLeftAngle, DriveConstants::BLOFF); m_frontRight.drive(-frontRightSpeed, frontRightAngle, - DriveConstants::FROFF); - m_frontLeft.drive(frontLeftSpeed, frontLeftAngle, - DriveConstants::FLOFF); + DriveConstants::FROFF); + m_frontLeft.drive(frontLeftSpeed, frontLeftAngle, DriveConstants::FLOFF); } - -//Updates the Odometry -void -SwerveDrive::UpdateOdometry(double theta){ - double BR_A = m_backRight.getAngle(DriveConstants::BROFF); - double BR_V = m_backRight.getVelocity(); - double BL_A = m_backLeft.getAngle(DriveConstants::BLOFF); - double BL_V = m_backLeft.getVelocity(); - double FR_A = m_frontRight.getAngle(DriveConstants::FROFF); - double FR_V = m_frontRight.getVelocity(); - double FL_A = m_frontLeft.getAngle(DriveConstants::FLOFF); - double FL_V = m_frontLeft.getVelocity(); - m_odometry.updateOdometry(BR_A, BR_V, BL_A, BL_V, FR_A, FR_V, - FL_A, FL_V, theta); +// Updates the Odometry +void SwerveDrive::UpdateOdometry(double dt) { + // NOTE I am explicitly ignoring any polarity flips, + // because those should be fixed elsewhere, not here. + // The offsets are sufficient to ensure that modules + // return robot-relative wheel velocities as (angle, vel) + // pairs. + double BR_A = m_backRight.getAngleRad(DriveConstants::BROFF); + double BR_D = m_backRight.getWheelDistance(); + double BL_A = m_backLeft.getAngleRad(DriveConstants::BLOFF); + double BL_D = m_backLeft.getWheelDistance(); + double FR_A = m_frontRight.getAngleRad(DriveConstants::FROFF); + double FR_D = m_frontRight.getWheelDistance(); + double FL_A = m_frontLeft.getAngleRad(DriveConstants::FLOFF); + double FL_D = m_frontLeft.getWheelDistance(); + m_odometry.updateOdometry(BR_A, BR_D, BL_A, BL_D, FR_A, FR_D, FL_A, FL_D, + GetGyroAngleRad(), dt); } - -//Trajectory Following for Swerve -//Cannot turn and move at the same time -//If you want to turn make Waypoint as 0,0,0, theta -void -SwerveDrive::TrajectoryFollow(double rot, size_t waypointIndex){ +// Trajectory Following for Swerve +// Cannot turn and move at the same time +// If you want to turn make Waypoint as 0,0,0, theta +void SwerveDrive::TrajectoryFollow(double rot, size_t waypointIndex) { size_t index = m_trajectory_1.getIndex(); - + bool YError = (abs(GetYPosition() - m_trajectory_1.getY(index)) < 0.3); - bool XError = (abs(GetXPostion() - m_trajectory_1.getX(index)) < 0.3); + bool XError = (abs(GetXPosition() - m_trajectory_1.getX(index)) < 0.3); bool RotError = (abs(rot - m_trajectory_1.getRotation(index)) < 5); - frc::SmartDashboard::PutNumber("RotError", (abs(rot - m_trajectory_1.getRotation(index)))); + frc::SmartDashboard::PutNumber( + "RotError", (abs(rot - m_trajectory_1.getRotation(index)))); frc::SmartDashboard::PutBoolean("ROTERROR", RotError); - if(m_trajectory_1.getX(index) == 0 && - m_trajectory_1.getY(index) == 0 && - m_trajectory_1.getRotation(index) != 0){ - if(RotError && index < waypointIndex){ + if (m_trajectory_1.getX(index) == 0 && m_trajectory_1.getY(index) == 0 && + m_trajectory_1.getRotation(index) != 0) { + if (RotError && index < waypointIndex) { m_trajectory_1.Progress(); std::cout << "Gets here once" << std::endl; gyro->Reset(); } - Drive(0, 0, calcYawStraight(m_trajectory_1.getRotation(index), rot), rot, true); + Drive(0, 0, calcYawStraight(m_trajectory_1.getRotation(index), rot), + rot, true); return; } - m_swerveController.updatePosition(GetYPosition(), GetXPostion(), rot); + m_swerveController.updatePosition(GetYPosition(), GetXPosition(), rot); - //frc::SmartDashboard::PutNumber("YError", abs(GetYPosition() - m_trajectory_1.getY(index))); - //frc::SmartDashboard::PutNumber("XError", abs(GetXPostion() - m_trajectory_1.getX(index))); + // frc::SmartDashboard::PutNumber("YError", abs(GetYPosition() - + // m_trajectory_1.getY(index))); frc::SmartDashboard::PutNumber("XError", + // abs(GetXPosition() - m_trajectory_1.getX(index))); - double forward = m_swerveController.calculateForward(m_trajectory_1.getY(index)); - double strafe = m_swerveController.calculateStrafe(m_trajectory_1.getX(index)); + double forward = + m_swerveController.calculateForward(m_trajectory_1.getY(index)); + double strafe = + m_swerveController.calculateStrafe(m_trajectory_1.getX(index)); - if( YError && XError && RotError && index < waypointIndex){ + if (YError && XError && RotError && index < waypointIndex) { m_trajectory_1.Progress(); } - //frc::SmartDashboard::PutNumber("strafe", strafe); + // frc::SmartDashboard::PutNumber("strafe", strafe); - UpdateOdometry(rot); - Drive(strafe, forward, calcYawStraight(m_trajectory_1.getRotation(index), rot), rot, true); + Drive(strafe, forward, + calcYawStraight(m_trajectory_1.getRotation(index), rot), rot, true); } +void SwerveDrive::TrajectoryFollow2(double x_d, double y_d, double theta_d, + double xdot_d, double ydot_d, + double thetadot_d) { + // We run PIDF controllers on each of x,y,theta independently. + // We run these controllers to be stateless, which simplified the code. + // Usually PID controllers are stateful due to integrator and differentiator + // state. We are not going to use any integral terms (raise your P and tune + // your paths instead) (though theres a slim chance integral is needed + // eventually) Differentiator state is handled through noting that D is + // equivalent to negative sensor rate feedback, which is stateless: + // + // output = P * poserror + D * d/dt poserror + F * pathvel + // = P * poserror + D * d/dt (pos - pathpos) + F * pathvel + // = P * poserror + D * vel - D * pathvel + F * pathvel + // = P * poserror + D * vel + (F-D) * pathvel + // + // So with a slight adjustment, we can make it work. + // Another improvement would be to add an acceleration feedforward. + + double xerr = x_d - GetXPosition(); + double yerr = y_d - GetXPosition(); + double terr = theta_d - GetGyroAngleRad(); + // bound angle error properly to (-pi, pi) + terr = terr - M_2_PI * round(terr / M_2_PI); + + double xvel = GetXSpeed(); + double yvel = GetXSpeed(); + double tvel = GetGyroVelRad(); + + using namespace DriveConstants; + double world_x_cmd = TRAJ_TRANSLATE_P * xerr + TRAJ_TRANSLATE_D * xvel + + TRAJ_TRANSLATE_F * xdot_d; + double world_y_cmd = TRAJ_TRANSLATE_P * yerr + TRAJ_TRANSLATE_D * yvel + + TRAJ_TRANSLATE_F * ydot_d; + double world_t_cmd = + TRAJ_ROT_P * terr + TRAJ_ROT_D * tvel + TRAJ_ROT_F * thetadot_d; + + // TODO: I am not sure if this argument order is correct. + Drive(world_y_cmd, world_x_cmd, world_t_cmd, gyro->GetYaw(), true); +} -//Function for generating trajectory_1 -void -SwerveDrive::GenerateTrajectory_1(){ +// Function for generating trajectory_1 +void SwerveDrive::GenerateTrajectory_1() { m_trajectory_1.clearTrajectory(); // Default Scoot and Shoot @@ -135,7 +191,7 @@ SwerveDrive::GenerateTrajectory_1(){ Trajectory::Waypoint p5(0, 0, 0, 70); m_trajectory_1.addWaypoint(p1); m_trajectory_1.addWaypoint(p2); - m_trajectory_1.addWaypoint(p3); + m_trajectory_1.addWaypoint(p3); m_trajectory_1.addWaypoint(p4); m_trajectory_1.addWaypoint(p5); @@ -148,109 +204,81 @@ SwerveDrive::GenerateTrajectory_1(){ // Trajectory::Waypoint p6(7.0, 1.4, 0, 0); // m_trajectory_1.addWaypoint(p1); // m_trajectory_1.addWaypoint(p2); - // m_trajectory_1.addWaypoint(p3); + // m_trajectory_1.addWaypoint(p3); // m_trajectory_1.addWaypoint(p4); // m_trajectory_1.addWaypoint(p5); // m_trajectory_1.addWaypoint(p6); } - -void -SwerveDrive::GenerateTrajectory_2(){ +void SwerveDrive::GenerateTrajectory_2() { m_trajectory_2.clearTrajectory(); // Trajectory::Waypoint p1(0, 0, 0, 90); - // m_trajectory_2.addWaypoint(p1); -} - - -//Return y position of the robot -double -SwerveDrive::GetYPosition(){ - return m_odometry.getY(); -} - - -//Return x position of the robot -double -SwerveDrive::GetXPostion(){ - return m_odometry.getX(); + // m_trajectory_2.addWaypoint(p1); } +// Return y position of the robot +double SwerveDrive::GetYPosition() { return m_odometry.getY(); } -double -SwerveDrive::GetYSpeed(){ - return m_odometry.getYSpeed(); -} - +// Return x position of the robot +double SwerveDrive::GetXPosition() { return m_odometry.getX(); } -double -SwerveDrive::GetXSpeed(){ - return m_odometry.getXSpeed(); -} +double SwerveDrive::GetYSpeed() { return m_odometry.getYSpeed(); } +double SwerveDrive::GetXSpeed() { return m_odometry.getXSpeed(); } -//Rotation correction - P controller -double -SwerveDrive::calcYawStraight(double targetAngle, double currentAngle){ +// Rotation correction - P controller +double SwerveDrive::calcYawStraight(double targetAngle, double currentAngle) { double error = targetAngle - currentAngle; - return 0.013*error; -} - - -//Function to set PID for Swerve Drive Controller -void -SwerveDrive::SetDriveControllerPID(){ - m_swerveController.setPID(); + return 0.013 * error; } +// Function to set PID for Swerve Drive Controller +void SwerveDrive::SetDriveControllerPID() { m_swerveController.setPID(); } -void -SwerveDrive::SetDriveControllerROTPID(){ - m_swerveController.setRotPID(); -} - +void SwerveDrive::SetDriveControllerROTPID() { m_swerveController.setRotPID(); } -//REDUNDANT!! FIX -//Function to Set PID for Swerve Modules -void -SwerveDrive::SetPID(){ +// REDUNDANT!! FIX +// Function to Set PID for Swerve Modules +void SwerveDrive::SetPID() { m_backRight.setPID(); m_backLeft.setPID(); m_frontRight.setPID(); m_frontLeft.setPID(); } +// Reset Odometry +void SwerveDrive::ResetOdometry() { m_odometry.Reset(); } -//Reset Odometry -void -SwerveDrive::ResetOdometry(){ - m_odometry.Reset(); -} - - -//For initialization of Swerve Modules -void -SwerveDrive::Initialize(){ +// For initialization of Swerve Modules +void SwerveDrive::Initialize() { m_backLeft.initialization(m_BL_Offset); m_backRight.initialization(m_BR_Offset); m_frontRight.initialization(m_FR_Offset); - m_frontLeft.initialization(m_FL_Offset); + m_frontLeft.initialization(m_FL_Offset); } - -//Reset Swerve Module Encoders -void -SwerveDrive::ResetEncoders(){ +// Reset Swerve Module Encoders +void SwerveDrive::ResetEncoders() { m_backLeft.resetEncoder(); m_backRight.resetEncoder(); m_frontLeft.resetEncoder(); m_frontRight.resetEncoder(); } +// Helper Function +void SwerveDrive::InjectNavx(AHRS *navx) { gyro = navx; } -//Helper Function -void -SwerveDrive::debug(AHRS &navx){ - gyro = &navx; -} \ No newline at end of file +double SwerveDrive::GetGyroAngleRad() { + assert(gyro != nullptr); + // flip direction, navx is CW around Z, not CCW + // but double flip since roborio is upsidedown + return gyro->GetYaw() * (M_PI / 180.0); +} + +double SwerveDrive::GetGyroVelRad() { + assert(gyro != nullptr); + // flip direction, navx is CW around Z, not CCW + // but double flip since roborio is upsidedown + return gyro->GetRawGyroZ() * (M_PI / 180.0); +} diff --git a/src/main/cpp/WheelDrive.cpp b/src/main/cpp/WheelDrive.cpp index 3e75b55..333e1a2 100644 --- a/src/main/cpp/WheelDrive.cpp +++ b/src/main/cpp/WheelDrive.cpp @@ -1,10 +1,9 @@ #include - -WheelDrive::WheelDrive(int angleMotorPort, int speedMotorPort, - int encoder) - : angleMotor(angleMotorPort, "Drivebase"), speedMotor(speedMotorPort, "Drivebase"), - m_canCoder(encoder, "Drivebase"){ +WheelDrive::WheelDrive(int angleMotorPort, int speedMotorPort, int encoder) + : angleMotor(angleMotorPort, "Drivebase"), + speedMotor(speedMotorPort, "Drivebase"), + m_canCoder(encoder, "Drivebase") { pidController.EnableContinuousInput(-180, 180); initializeController.EnableContinuousInput(-180, 180); @@ -14,100 +13,93 @@ WheelDrive::WheelDrive(int angleMotorPort, int speedMotorPort, angleMotor.SetSafetyEnabled(false); speedMotor.SetSafetyEnabled(false); - // m_canCoder.SetStatusFramePeriod(CANCoderStatusFrame::CANCoderStatusFrame_SensorData, 20, 100); + // m_canCoder.SetStatusFramePeriod(CANCoderStatusFrame::CANCoderStatusFrame_SensorData, + // 20, 100); m_canCoder.ClearStickyFaults(); } - -//Drive function, set each module's speed and angle -void -WheelDrive::drive(double speed, double angle, double offSet){ +// Drive function, set each module's speed and angle +void WheelDrive::drive(double speed, double angle, double offSet) { double value = getAngle(offSet); - if(abs(value - angle) > 90){ - angle = (angle > 0 )? angle - 180: angle + 180; + if (abs(value - angle) > 90) // TODO: this doesn't handle wrapping behavior + { + angle = (angle > 0) ? angle - 180 : angle + 180; m_reverse = true; - } - else { + } else { m_reverse = false; } - - double turnOutput = std::clamp(pidController.Calculate(value, angle),-0.5, 0.5); - - m_speedOut = (m_reverse)? -1*0.87*speed: 0.87*speed; - + + double turnOutput = + std::clamp(pidController.Calculate(value, angle), -0.5, 0.5); + + m_speedOut = (m_reverse) ? -1 * 0.87 * speed : 0.87 * speed; + speedMotor.Set(m_speedOut); angleMotor.Set(turnOutput); } - -//Converts Encoder Value to -180, +180 -double -WheelDrive::normalizeEncoderValue(){ +// Converts Encoder Value to -180, +180 +double WheelDrive::normalizeEncoderValue() { m_currEncoderValue = m_canCoder.GetPosition(); - int encoderDelta = -1*(m_currEncoderValue - m_prevEncoderValue); - m_angle += (double) encoderDelta; - m_angle = (m_angle > 360) ? m_angle-360: m_angle; - m_angle = (m_angle < 0) ? m_angle+360 : m_angle; + int encoderDelta = -1 * (m_currEncoderValue - m_prevEncoderValue); + m_angle += (double)encoderDelta; + m_angle = (m_angle > 360) ? m_angle - 360 : m_angle; + m_angle = (m_angle < 0) ? m_angle + 360 : m_angle; m_prevEncoderValue = m_currEncoderValue; - m_angle = (m_angle > 180)? m_angle - 360: m_angle; + m_angle = (m_angle > 180) ? m_angle - 360 : m_angle; return m_angle; } - -//Function to set the PID for Swerve Modules -void -WheelDrive::setPID(){ +// Function to set the PID for Swerve Modules +void WheelDrive::setPID() { double pGain_a = frc::SmartDashboard::GetNumber("P angle", 0.0); frc::SmartDashboard::PutNumber("P angle", pGain_a); double iGain_a = frc::SmartDashboard::GetNumber("I angle", 0.0); frc::SmartDashboard::PutNumber("I angle", iGain_a); double dGain_a = frc::SmartDashboard::GetNumber("D angle", 0.0); frc::SmartDashboard::PutNumber("D angle", dGain_a); - //initializeController.SetPID(pGain_a, iGain_a, dGain_a); + // initializeController.SetPID(pGain_a, iGain_a, dGain_a); pidController.SetPID(pGain_a, iGain_a, dGain_a); } +// Reset encoder curruntly not used +void WheelDrive::resetEncoder() { m_reverse = false; } -//Reset encoder curruntly not used -void -WheelDrive::resetEncoder(){ - m_reverse = false; +// Returns the velocity of Swerve Module in ft per 0.02s +double WheelDrive::getVelocity() { + return speedMotor.GetSelectedSensorVelocity() / 2048 * (1 / 6.12) * 4.375 * + PI / 5 / 12; } - -//Returns the velocity of Swerve Module in ft per 0.02s -double -WheelDrive::getVelocity(){ - return speedMotor.GetSelectedSensorVelocity()/2048 * (1/6.12) * 4.375 * PI / 5/ 12; +double WheelDrive::getWheelDistance() { + return speedMotor.GetSelectedSensorPosition() * + DriveConstants::EMPIRIC_WHEEL_METERS_PER_TICK; } - -//Returns the angle of Swerve Module -double -WheelDrive::getAngle(double offset){ +// Returns the angle of Swerve Module, in radians +double WheelDrive::getAngle(double offset) { return m_canCoder.GetAbsolutePosition() - offset; } +double WheelDrive::getAngleRad(double offset) { + return getAngle(offset) * (M_PI / 180.0); +} -//Initializes the zero position of Swerve Module -void -WheelDrive::initialization(double Offset){ +// Initializes the zero position of Swerve Module +void WheelDrive::initialization(double Offset) { m_reverse = false; - //double turnOutput = std::clamp(initializeController.Calculate(getAngle(Offset), Offset), -0.5, 0.5); - //angleMotor.Set(turnOutput); + // double turnOutput = + // std::clamp(initializeController.Calculate(getAngle(Offset), Offset), + // -0.5, 0.5); angleMotor.Set(turnOutput); } - -//Helper function -void -WheelDrive::Debug(){ - //frc::SmartDashboard::PutNumber("relative", normalizeEncoderValue()); - //frc::SmartDashboard::PutNumber("absolute", GetabsAngle()); +// Helper function +void WheelDrive::Debug() { + // frc::SmartDashboard::PutNumber("relative", normalizeEncoderValue()); + // frc::SmartDashboard::PutNumber("absolute", GetabsAngle()); } - -void -WheelDrive::Stop(){ +void WheelDrive::Stop() { angleMotor.Set(ControlMode::PercentOutput, 0.0); speedMotor.Set(ControlMode::PercentOutput, 0.0); -} \ No newline at end of file +} diff --git a/src/main/include/AutoMode2.h b/src/main/include/AutoMode2.h new file mode 100644 index 0000000..8761c4d --- /dev/null +++ b/src/main/include/AutoMode2.h @@ -0,0 +1,96 @@ +#pragma once + +#include "Intake.h" +#include "Shooter.h" +#include "SwerveDrive.h" +#include "Traj2.h" +#include "Trajectories.h" + +class AutoMode2 { + public: + AutoMode2(SwerveDrive* swerve, Shooter* shooter, Intake* intake) { + m_swerve = swerve; + m_shooter = shooter; + m_intake = intake; + m_autostate = -1; + } + + virtual void tick(double time) = 0; + void start(double time) { m_autostate = 0; }; + + protected: + // utilities for waiting + virtual void beginWait(double time) { m_wait_start = time; } + + virtual bool hasElapsed(double timeout, double time) { + return time - m_wait_start > timeout; + } + // Non-owning pointers to robot subsystems to run the mode + SwerveDrive* m_swerve; + Shooter* m_shooter; + Intake* m_intake; + int m_autostate{0}; + + double m_wait_start{0.0}; +}; + +class TwoBallAuto : public AutoMode2 { + public: + TwoBallAuto(SwerveDrive* swerve, Shooter* shooter, Intake* intake) + : AutoMode2(swerve, shooter, intake) { + segment1 = trajectories::twoball_segment1(); + } + + virtual void tick(double time) override { + switch (m_autostate) { + case 0: + m_intake->setState(Intake::IDLE); + m_shooter->setState(Shooter::IDLE); + m_swerve->Drive(0, 0, 0, 0, true); + beginWait(time); + m_autostate++; + break; + case 1: + if (hasElapsed(0.5, time)) m_autostate++; + break; + case 2: + m_shooter->setState(Shooter::SHOOT); + beginWait(time); + m_autostate++; + break; + case 3: + if (hasElapsed(0.5, time)) { + m_shooter->setState(Shooter::IDLE); + m_autostate++; + } + break; + case 4: + // Intake and path follow + m_intake->setState(Intake::RUN); + m_shooter->setState(Shooter::LOAD); + segment1->start(time); + m_autostate++; + break; + case 5: + if (segment1->is_done(time)) { + m_autostate++; + } + segment1->drive(time, m_swerve); + break; + case 6: + m_swerve->Drive(0, 0, 0, 0, true); + m_shooter->setState(Shooter::SHOOT); + beginWait(time); + m_autostate++; + break; + case 7: + if (hasElapsed(0.5, time)) m_autostate++; + break; + case 8: + m_shooter->setState(Shooter::IDLE); + break; + } + } + + std::unique_ptr segment1; +}; diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index b1e67ba..04f99f9 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -1,154 +1,167 @@ #pragma once - -namespace -GeneralConstants{ - const double timeStep = 0.02; - - //In meters - const double goalHeight = 2.641; - const double cameraHeight = 0.4572; - const double cameraPitch = 50; -} - - -namespace -OIConstants{ - //Joystick & xbox input ports - const int l_joy_port = 0; - const int r_joy_port = 1; - const int O_joy_port = 2; +/* +Coordinate Frames: + + INTAKE +|O ====== O| +| | +x ^ +| X | | +| | +y | +|O O| <-----+ + + +Angles are measured CCW from +x to +y, per right hand standard. +X marks the orign of the robot frame. The robot coordinate frame moves with the +robot. The world frame is equal to the robot frame at t=0, and is fixed. +*/ + +#define PI M_PI + +namespace GeneralConstants { +const double timeStep = 0.02; + +// In meters +const double goalHeight = 2.641; +const double cameraHeight = 0.4572; +const double cameraPitch = 50; +} // namespace GeneralConstants + +namespace OIConstants { +// Joystick & xbox input ports +const int l_joy_port = 0; +const int r_joy_port = 1; +const int O_joy_port = 2; +} // namespace OIConstants + +namespace DriveConstants { +// Dimensions of the Robot - SwerveDrive.h +const double Width = 29; +const double Length = 29; + +// Absolute Encoder offsets +const double FLOFF = -108.5; +const double FROFF = 157.0; +const double BLOFF = 7.91; +const double BROFF = -18.98; + +// Ports for Back Right Swerve Module - SwerveDrive.h +const int BRanglePort = 18; +const int BRspeedPort = 17; +const int BRencoder = 8; + +// Ports for Back Left Swerve Module - SwerveDrive.h +const int BLanglePort = 16; +const int BLspeedPort = 15; +const int BLencoder = 42; + +// Ports for Front Right Swerve Module - SwerveDrive.h +const int FRanglePort = 14; +const int FRspeedPort = 13; +const int FRencoder = 62; + +// Ports for Front Left Swerve Module - SwerveDrive.h +const int FLanglePort = 12; +const int FLspeedPort = 11; +const int FLencoder = 10; + +// Wheeldrive +constexpr double MAX_VOLTS = 1.0; +const double speedGearRatio = 1 / 6.12; +const double angleGearRatio = 1 / 12.8; +const double P = 0.005; +const double I = 0.001; +const double D = 0.0002; +const double Pinit = 0.008; +const double Iinit = 0.001; +const double Dinit = 0.0001; + +// Swerve Controller +const double fwdstrP = 0.58; +const double fwdstrI = 0.0; +const double fwdstrD = 0.0; +const double rotP = 0.0011; +const double rotI = 0.00006; +const double rotD = 0.0; + +const double EMPIRIC_WHEEL_METERS_PER_TICK = 1.0; // TODO: TUNE +const double TRAJ_TRANSLATE_P = 1.0; // TODO: TUNE +const double TRAJ_TRANSLATE_D = 1.0; // TODO: TUNE +const double TRAJ_TRANSLATE_F = 1.0; // TODO: TUNE +const double TRAJ_ROT_P = 1.0; // TODO: TUNE +const double TRAJ_ROT_D = 1.0; // TODO: TUNE +const double TRAJ_ROT_F = 1.0; // TODO: TUNE + +const double MAX_TRAJ_WHEEL_VELOCITY_MPS = 3.6; // TODO: TUNE +} // namespace DriveConstants + +// None of these values have been set yet +namespace ShooterConstants { + +// TalonFX MotorID +const int shootMotorPortMaster = 22; // confirmed +const int shootMotorPortSlave = 23; // confirmed +const int turretMotorPort = 20; // confirmed +const int hoodMotorPort = 19; // confirmed +const int kickerMotorPort = 21; // confirmed + +// Limit Switch +const int turretLimitSwitch = 9; // confirmed + +// Photogate +const int photogate = 8; + +// Turret Controller +const double turretP = 0.022; // fairly good +const double turretI = 0.0007; +const double turretD = 0.0012; + +// Flywheel Controller +const double flywheelF = 0.049; +const double flywheelP = 0.045; +const double flywheelI = -0.000014; +const double flywheelD = 0.002; + +// Hood Controller +const double hoodP = 0.051; +const double hoodI = 0.00016; +const double hoodD = 0.0; +const double hoodF = 0.0; + +// Turret limit values; +const double turretMax = -1000.0; // confirmed +const double turretMin = -63000.0; // confirmed + +const double hoodMax = 5700; // confirmed +const double hoodMin = 0; // confirmed + +const double angleOff = 300; + +const double zeroingcurrent = 1.75; // confirmed + +const double TalonFXunitsperrot = 2048; +} // namespace ShooterConstants + +namespace IntakeConstants { +const int intakeMotorPort = 50; // confirmed + +const int solenoidPort = 0; +} // namespace IntakeConstants + +// None of these values are set yet +namespace ClimbConstants { +const int gearboxPort1 = 30; // confirmed +const int gearboxPort2 = 31; // confirmed +const int solenoid1Port = 2; +const int solenoid2Port = 3; +const int diskBrakePort = 1; +} // namespace ClimbConstants + +namespace ChannelConstants { +const int channelMotorPort = 40; // confirmed } - -namespace -DriveConstants{ - //Dimensions of the Robot - SwerveDrive.h - const double Width = 29; - const double Length = 29; - - //Absolute Encoder offsets - const double FLOFF = -108.5; - const double FROFF = 157.0; - const double BLOFF = 7.91; - const double BROFF = -18.98; - - //Ports for Back Right Swerve Module - SwerveDrive.h - const int BRanglePort = 18; - const int BRspeedPort = 17; - const int BRencoder = 8; - - //Ports for Back Left Swerve Module - SwerveDrive.h - const int BLanglePort = 16; - const int BLspeedPort = 15; - const int BLencoder = 42; - - //Ports for Front Right Swerve Module - SwerveDrive.h - const int FRanglePort = 14; - const int FRspeedPort = 13; - const int FRencoder = 62; - - //Ports for Front Left Swerve Module - SwerveDrive.h - const int FLanglePort = 12; - const int FLspeedPort = 11; - const int FLencoder = 10; - - //Wheeldrive - constexpr double MAX_VOLTS = 1.0; - const double speedGearRatio = 1/6.12; - const double angleGearRatio = 1/12.8; - const double P = 0.005; - const double I = 0.001; - const double D = 0.0002; - const double Pinit = 0.008; - const double Iinit = 0.001; - const double Dinit = 0.0001; - - //Swerve Controller - const double fwdstrP = 0.58; - const double fwdstrI = 0.0; - const double fwdstrD = 0.0; - const double rotP = 0.0011; - const double rotI = 0.00006; - const double rotD = 0.0; -} - - -//None of these values have been set yet -namespace -ShooterConstants{ - - //TalonFX MotorID - const int shootMotorPortMaster = 22; //confirmed - const int shootMotorPortSlave = 23; //confirmed - const int turretMotorPort = 20; //confirmed - const int hoodMotorPort = 19; //confirmed - const int kickerMotorPort = 21; //confirmed - - //Limit Switch - const int turretLimitSwitch = 9; //confirmed - - //Photogate - const int photogate = 8; - - //Turret Controller - const double turretP = 0.022; //fairly good - const double turretI = 0.0007; - const double turretD = 0.0012; - - //Flywheel Controller - const double flywheelF = 0.049; - const double flywheelP = 0.045; - const double flywheelI = -0.000014; - const double flywheelD = 0.002; - - //Hood Controller - const double hoodP = 0.051; - const double hoodI = 0.00016; - const double hoodD = 0.0; - const double hoodF = 0.0; - - //Turret limit values; - const double turretMax = -1000.0; //confirmed - const double turretMin = -63000.0; //confirmed - - const double hoodMax = 5700; //confirmed - const double hoodMin = 0; //confirmed - - const double angleOff = 300; - - const double zeroingcurrent = 1.75; //confirmed - - const double TalonFXunitsperrot = 2048; -} - - -namespace -IntakeConstants{ - const int intakeMotorPort = 50; //confirmed - - const int solenoidPort = 0; -} - - -//None of these values are set yet -namespace -ClimbConstants{ - const int gearboxPort1 = 30; //confirmed - const int gearboxPort2 = 31; //confirmed - const int solenoid1Port = 2; - const int solenoid2Port = 3; - const int diskBrakePort = 1; -} - - -namespace -ChannelConstants{ - const int channelMotorPort = 40; //confirmed -} - - -//0 is intkae -//1 is disk brake for climber gearbox -// 2 is climb first -// 3 is climb 2nd stage \ No newline at end of file +// 0 is intkae +// 1 is disk brake for climber gearbox +// 2 is climb first +// 3 is climb 2nd stage diff --git a/src/main/include/Odometry.h b/src/main/include/Odometry.h index 155d488..6cd8887 100644 --- a/src/main/include/Odometry.h +++ b/src/main/include/Odometry.h @@ -1,39 +1,32 @@ #pragma once -#include -#include #include +#include +#include -#define PI 3.14159265 - -class Odometry{ - - public: - Odometry(); - void updateOdometry(double BR_A, double BR_V, - double BL_A, double BL_V, - double FR_A, double FR_V, - double FL_A, double FL_V, - double theta); - double getX(); - double getY(); - double getXSpeed(); - double getYSpeed(); - void Reset(); - - - private: - const double timeInterval = GeneralConstants::timeStep; - double m_timeStep = 0; - double m_prevtimeStep = 0; - - double m_x = 0; - double m_y = 0; - double m_L = DriveConstants::Length; - double m_W = DriveConstants::Width; - - double m_FWD; - double m_STR; - double m_FWD_new; - double m_STR_new; -}; \ No newline at end of file +class Odometry { + public: + Odometry(); + void updateOdometry(double BR_A, double BR_V, double BL_A, double BL_V, + double FR_A, double FR_V, double FL_A, double FL_V, + double theta, double dt); + double getX(); + double getY(); + double getXSpeed(); + double getYSpeed(); + void Reset(); + + private: + const double timeInterval = GeneralConstants::timeStep; + double m_timeStep = 0; + double m_prevtimeStep = 0; + + double m_x = 0; + double m_y = 0; + + double m_xdot = 0; + double m_ydot = 0; + + double init = false; + double prev_bl{0.0}, prev_br{0.0}, prev_fl{0.0}, prev_fr{0.0}; +}; diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index c46db27..0564ac8 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -1,25 +1,27 @@ #pragma once -#include -#include -#include -#include +#include +#include #include +#include +#include +#include #include -#include #include -#include +#include #include -#include -#include -#include +#include +#include + +#include + +#include "AutoMode.h" +#include "AutoMode2.h" +#include "Climber.h" #include "Intake.h" #include "Shooter.h" -#include "Climber.h" -#include "AutoMode.h" #include "cameraserver/CameraServer.h" - frc::Joystick l_joy{OIConstants::l_joy_port}; frc::Joystick r_joy{OIConstants::r_joy_port}; frc::XboxController xbox{OIConstants::O_joy_port}; @@ -29,33 +31,36 @@ cs::UsbCamera camera; double m_time = 0; double m_timeStep = GeneralConstants::timeStep; -AHRS *navx; - class Robot : public frc::TimedRobot { - public: - void RobotInit() override; - void RobotPeriodic() override; - void AutonomousInit() override; - void AutonomousPeriodic() override; - void TeleopInit() override; - void TeleopPeriodic() override; - void DisabledInit() override; - void DisabledPeriodic() override; - void TestInit() override; - void TestPeriodic() override; - - private: - SwerveDrive m_swerve; - AutoMode m_auto; - Intake m_intake; - Shooter m_shooter; - Climber m_climber; - - double out; - bool m_climbing = false; - - frc::SendableChooser m_chooser; - const std::string blueAlliance = "BLUE"; - const std::string redAlliance = "RED"; - std::string m_autoSelected; -}; \ No newline at end of file + public: + void RobotInit() override; + void RobotPeriodic() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void TestInit() override; + void TestPeriodic() override; + + double GetGyroAngleRad(); + + private: + SwerveDrive m_swerve; + AutoMode m_auto; + Intake m_intake; + Shooter m_shooter; + Climber m_climber; + AHRS *navx; + + double out; + bool m_climbing = false; + + frc::SendableChooser m_chooser; + const std::string blueAlliance = "BLUE"; + const std::string redAlliance = "RED"; + std::string m_autoSelected; + + // std::unique_ptr m_automode; +}; diff --git a/src/main/include/SwerveDrive.h b/src/main/include/SwerveDrive.h index 02896bb..eda2bc8 100644 --- a/src/main/include/SwerveDrive.h +++ b/src/main/include/SwerveDrive.h @@ -1,65 +1,72 @@ #pragma once -#include -#include "math.h" -#include -#include +#include +#include #include +#include #include -#include -#include - +#include +#include -class SwerveDrive{ +#include "math.h" - public: - SwerveDrive(); - void Drive (double x1, double y1, double x2, double rot, bool fieldOrient); - void SetPID(); - void TrajectoryFollow(double rot, size_t waypointIndex = 9999); - void SetDriveControllerPID(); - void SetDriveControllerROTPID(); - void UpdateOdometry(double theta); - void ResetOdometry(); - void Initialize(); - void ResetEncoders(); - double GetYPosition(); - double GetXPostion(); - double GetYSpeed(); - double GetXSpeed(); - double calcYawStraight(double targetAngle, double currentAngle); - void GenerateTrajectory_1(); - void GenerateTrajectory_2(); +class SwerveDrive { + public: + SwerveDrive(); + void Drive(double x1, double y1, double x2, double rot, bool fieldOrient); + void SetPID(); + void TrajectoryFollow(double rot, size_t waypointIndex = 9999); + void TrajectoryFollow2(double x_d, double y_d, double theta_d, + double xdot_d, double ydot_d, double thetadot_d); + void SetDriveControllerPID(); + void SetDriveControllerROTPID(); + void UpdateOdometry(double dt); + void ResetOdometry(); + void Initialize(); + void ResetEncoders(); + double GetYPosition(); + double GetXPosition(); + double GetYSpeed(); + double GetXSpeed(); + double calcYawStraight(double targetAngle, double currentAngle); + void GenerateTrajectory_1(); + void GenerateTrajectory_2(); - void debug(AHRS &navx); + void InjectNavx(AHRS *navx); + private: + double GetGyroAngleRad(); + double GetGyroVelRad(); - private: - double m_W = DriveConstants::Width; - double m_L = DriveConstants::Length; + double m_W = DriveConstants::Width; + double m_L = DriveConstants::Length; - double m_temp; + double m_temp; - double m_FL_Offset = DriveConstants::FLOFF; - double m_FR_Offset = DriveConstants::FROFF; - double m_BL_Offset = DriveConstants::BLOFF; - double m_BR_Offset = DriveConstants::BROFF; + double m_FL_Offset = DriveConstants::FLOFF; + double m_FR_Offset = DriveConstants::FROFF; + double m_BL_Offset = DriveConstants::BLOFF; + double m_BR_Offset = DriveConstants::BROFF; - WheelDrive m_backRight{DriveConstants::BRanglePort, - DriveConstants::BRspeedPort, DriveConstants::BRencoder}; - WheelDrive m_backLeft{DriveConstants::BLanglePort, - DriveConstants::BLspeedPort, DriveConstants::BLencoder}; - WheelDrive m_frontRight{DriveConstants::FRanglePort, - DriveConstants::FRspeedPort, DriveConstants::FRencoder}; - WheelDrive m_frontLeft{DriveConstants::FLanglePort, - DriveConstants::FLspeedPort, DriveConstants::FLencoder}; + WheelDrive m_backRight{DriveConstants::BRanglePort, + DriveConstants::BRspeedPort, + DriveConstants::BRencoder}; + WheelDrive m_backLeft{DriveConstants::BLanglePort, + DriveConstants::BLspeedPort, + DriveConstants::BLencoder}; + WheelDrive m_frontRight{DriveConstants::FRanglePort, + DriveConstants::FRspeedPort, + DriveConstants::FRencoder}; + WheelDrive m_frontLeft{DriveConstants::FLanglePort, + DriveConstants::FLspeedPort, + DriveConstants::FLencoder}; - SwerveController m_swerveController; + SwerveController m_swerveController; - Odometry m_odometry; + Odometry m_odometry; - Trajectory m_trajectory_1; - Trajectory m_trajectory_2; + Trajectory m_trajectory_1; + Trajectory m_trajectory_2; - AHRS *gyro; -}; \ No newline at end of file + AHRS *gyro; +}; diff --git a/src/main/include/Traj2.h b/src/main/include/Traj2.h new file mode 100644 index 0000000..1fcf065 --- /dev/null +++ b/src/main/include/Traj2.h @@ -0,0 +1,162 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +#ifndef PATH_TEST_PROGRAM +#include "SwerveDrive.h" +#endif + +struct Cubic3DUnitSpline { + double p0, v0, p1, v1; + double t0, t1; + + double at(double t) { + double dt = t1 - t0; + double tt = (t - t0) / dt; + double h00 = (1 + 2 * tt) * (1 - tt) * (1 - tt); + double h10 = tt * (1 - tt) * (1 - tt); + double h01 = tt * tt * (3 - 2 * tt); + double h11 = tt * tt * (tt - 1); + + double a = h00 * p0 + h10 * dt * v0 + h01 * p1 + h11 * dt * v1; + return a; + } + + double deriv_at(double t) { + double dt = t1 - t0; + double tt = (t - t0) / dt; + double tt2 = tt * tt; + double dh00 = 6 * tt2 - 6 * tt; + double dh10 = 3 * tt2 - 4 * tt + 1; + double dh01 = -6 * tt2 + 6 * tt; + double dh11 = 3 * tt2 - 2 * tt; + + double a = dh00 * p0 + dh10 * dt * v0 + dh01 * p1 + dh11 * dt * v1; + return a; + } +}; + +class Traj1D { + public: + Traj1D(std::vector> pts) { + assert(!pts.empty()); + assert(abs(std::get<2>(pts.at(0))) < 0.001); + + for (size_t i = 0; i + 1 < pts.size(); i++) { + Cubic3DUnitSpline spline; + spline.p0 = std::get<0>(pts.at(i)); + spline.v0 = std::get<1>(pts.at(i)); + spline.t0 = std::get<2>(pts.at(i)); + spline.p1 = std::get<0>(pts.at(i + 1)); + spline.v1 = std::get<1>(pts.at(i + 1)); + spline.t1 = std::get<2>(pts.at(i + 1)); + assert(spline.t1 > spline.t0); + splines.push_back(spline); + } + } + + void start(double time) { + spline_idx = 0; + time_offset = time; + started = true; + } + + std::pair continue_get_pos_vel(double time) { + assert(started); + time -= time_offset; + if (spline_idx >= splines.size()) { + auto& spline = splines.back(); + return std::make_pair(spline.p1, spline.v1); + } + auto* spline = &splines[spline_idx]; + while (time > spline->t1) { + spline_idx += 1; + if (spline_idx >= splines.size()) { + return continue_get_pos_vel(9999999999.9); + } + spline = &splines[spline_idx]; + } + return std::make_pair(spline->at(time), spline->deriv_at(time)); + } + + bool is_done(double time) { + return started && spline_idx >= splines.size(); + } + + private: + std::vector splines; + size_t spline_idx = 0; + double time_offset{0.0}; + bool started = false; +}; + +class RobotTraj { + public: + RobotTraj(Traj1D xx, Traj1D yy, Traj1D tt) + : x{std::move(xx)}, y{std::move(yy)}, theta{std::move(tt)} { + // Perform some basic verification of the path + double faketime = 0.0; + x.start(faketime); + y.start(faketime); + theta.start(faketime); + + double r = + 0.5 * 0.0254 * hypot(DriveConstants::Width, DriveConstants::Length); + + while (!(x.is_done(faketime) && y.is_done(faketime) && + theta.is_done(faketime))) { + double xvel = x.continue_get_pos_vel(faketime).second; + double yvel = y.continue_get_pos_vel(faketime).second; + double omega = theta.continue_get_pos_vel(faketime).second; + + double trans_vel = sqrt(xvel * xvel + yvel * yvel); + + double tangential_vel = abs(omega) * r; + + double max_possible_vel = trans_vel + tangential_vel; + if (max_possible_vel > + DriveConstants::MAX_TRAJ_WHEEL_VELOCITY_MPS) { + std::cout << "failure at time " << faketime << std::endl; + assert(max_possible_vel < + DriveConstants::MAX_TRAJ_WHEEL_VELOCITY_MPS); + } + faketime += 0.1; + } + + // Reset after verification + x.start(0.0); + y.start(0.0); + theta.start(0.0); + } + + void start(double time) { + x.start(time); + y.start(time); + theta.start(time); + } + +#ifndef PATH_TEST_PROGRAM + void drive(double time, SwerveDrive* sd) { + auto xd = x.continue_get_pos_vel(time); + auto yd = y.continue_get_pos_vel(time); + auto td = theta.continue_get_pos_vel(time); + + sd->TrajectoryFollow2(xd.first, yd.first, td.first, xd.second, + yd.second, td.second); + } +#endif + bool is_done(double time) { + return x.is_done(time) && y.is_done(time) && theta.is_done(time); + } + + Traj1D x; + Traj1D y; + Traj1D theta; +}; diff --git a/src/main/include/Trajectories.h b/src/main/include/Trajectories.h new file mode 100644 index 0000000..1b1dd39 --- /dev/null +++ b/src/main/include/Trajectories.h @@ -0,0 +1,31 @@ +#pragma once + +#include "Traj2.h" + +namespace trajectories { + +std::unique_ptr twoball_segment1() { + // Each axis gets a list of (pos, vel, time) tuples to define its + // trajectory + // (dunno if any of these values are right, but hopefully you get the + // idea) + auto seg1x = Traj1D({ + std::make_tuple(0.0, 0.0, 0.0), + std::make_tuple(-0.5, -0.5, 1.5), + std::make_tuple(-1.0, 0.0, 3.0), + // hold the end, the traj runs for the longest of the three: + std::make_tuple(-1.0, 0.0, 3.5), + }); + auto seg1y = Traj1D({ + std::make_tuple(0.0, 0.0, 0.0), + std::make_tuple(-0.5, 0.0, 1.5), + std::make_tuple(-1.0, 0.0, 3.0), + }); + auto seg1t = Traj1D({ + std::make_tuple(0.0, 0.0, 0.0), + std::make_tuple(M_PI, 0.0, 1.0), + }); + return std::make_unique(seg1x, seg1y, seg1t); +} + +} // namespace trajectories diff --git a/src/main/include/WheelDrive.h b/src/main/include/WheelDrive.h index bf848e1..0e5e1e8 100644 --- a/src/main/include/WheelDrive.h +++ b/src/main/include/WheelDrive.h @@ -1,51 +1,51 @@ #pragma once +#include #include -#include -#include -#include #include +#include #include -#include +#include +#include #include -#include +#include + +class WheelDrive { + public: + // WheelDrive(int angleMotorPort, int speedMotorPort, + // int encoderPortA, int encoderPortB, int pwmPort); + WheelDrive(int angleMotorPort, int speedMotorPort, int encoderPort); + void drive(double speed, double angle, double offSet); + void setPID(); + double normalizeEncoderValue(); + double getVelocity(); + double getWheelDistance(); + double getAngle(double offset); + double getAngleRad(double offset); + void resetEncoder(); + void initialization(double Offset); + void Debug(); + void Stop(); + + private: + static constexpr double MAX_VOLTS = DriveConstants::MAX_VOLTS; + + int m_prevEncoderValue = 0; + int m_currEncoderValue = 0; + double m_angle = 0; + bool m_reverse = false; + double m_speedOut = 0; + + double speedGearRatio = DriveConstants::speedGearRatio; + double angleGearRatio = DriveConstants::angleGearRatio; + + WPI_TalonFX angleMotor; + WPI_TalonFX speedMotor; + + WPI_CANCoder m_canCoder; -#define PI 3.14159265 - -class WheelDrive{ - public: - // WheelDrive(int angleMotorPort, int speedMotorPort, - // int encoderPortA, int encoderPortB, int pwmPort); - WheelDrive(int angleMotorPort, int speedMotorPort, - int encoderPort); - void drive(double speed, double angle, double offSet); - void setPID(); - double normalizeEncoderValue(); - double getVelocity(); - double getAngle(double offset); - void resetEncoder(); - void initialization(double Offset); - void Debug(); - void Stop(); - - private: - static constexpr double MAX_VOLTS = DriveConstants::MAX_VOLTS; - - int m_prevEncoderValue = 0; - int m_currEncoderValue = 0; - double m_angle = 0; - bool m_reverse = false; - double m_speedOut = 0; - - double speedGearRatio = DriveConstants::speedGearRatio; - double angleGearRatio = DriveConstants::angleGearRatio; - - WPI_TalonFX angleMotor; - WPI_TalonFX speedMotor; - - WPI_CANCoder m_canCoder; - - frc2::PIDController pidController{DriveConstants::P , DriveConstants::I, DriveConstants::D}; - frc2::PIDController initializeController{DriveConstants::Pinit, - DriveConstants::Iinit, DriveConstants::Dinit}; -}; \ No newline at end of file + frc2::PIDController pidController{DriveConstants::P, DriveConstants::I, + DriveConstants::D}; + frc2::PIDController initializeController{ + DriveConstants::Pinit, DriveConstants::Iinit, DriveConstants::Dinit}; +}; diff --git a/src/test/cpp/main.cpp b/src/test/cpp/main.cpp index cab8aa4..e254c8f 100644 --- a/src/test/cpp/main.cpp +++ b/src/test/cpp/main.cpp @@ -3,8 +3,8 @@ #include "gtest/gtest.h" int main(int argc, char** argv) { - HAL_Initialize(500, 0); - ::testing::InitGoogleTest(&argc, argv); - int ret = RUN_ALL_TESTS(); - return ret; + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; }