Feat/actual claw

.vscode/settings.json

@@ -14,5 +14,78 @@
     "**/.factorypath": true,
     "**/*~": true
   },
-  "C_Cpp.default.configurationProvider": "vscode-wpilib"
+  "C_Cpp.default.configurationProvider": "vscode-wpilib",
+  "files.associations": {
+    "array": "cpp",
+    "atomic": "cpp",
+    "bit": "cpp",
+    "*.tcc": "cpp",
+    "bitset": "cpp",
+    "cctype": "cpp",
+    "cfenv": "cpp",
+    "chrono": "cpp",
+    "clocale": "cpp",
+    "cmath": "cpp",
+    "codecvt": "cpp",
+    "compare": "cpp",
+    "complex": "cpp",
+    "concepts": "cpp",
+    "condition_variable": "cpp",
+    "cstdarg": "cpp",
+    "cstddef": "cpp",
+    "cstdint": "cpp",
+    "cstdio": "cpp",
+    "cstdlib": "cpp",
+    "cstring": "cpp",
+    "ctime": "cpp",
+    "cwchar": "cpp",
+    "cwctype": "cpp",
+    "deque": "cpp",
+    "list": "cpp",
+    "map": "cpp",
+    "set": "cpp",
+    "string": "cpp",
+    "unordered_map": "cpp",
+    "unordered_set": "cpp",
+    "vector": "cpp",
+    "exception": "cpp",
+    "algorithm": "cpp",
+    "functional": "cpp",
+    "iterator": "cpp",
+    "memory": "cpp",
+    "memory_resource": "cpp",
+    "numeric": "cpp",
+    "optional": "cpp",
+    "random": "cpp",
+    "ratio": "cpp",
+    "regex": "cpp",
+    "string_view": "cpp",
+    "system_error": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "utility": "cpp",
+    "fstream": "cpp",
+    "future": "cpp",
+    "initializer_list": "cpp",
+    "iomanip": "cpp",
+    "iosfwd": "cpp",
+    "iostream": "cpp",
+    "istream": "cpp",
+    "limits": "cpp",
+    "mutex": "cpp",
+    "new": "cpp",
+    "numbers": "cpp",
+    "ostream": "cpp",
+    "semaphore": "cpp",
+    "span": "cpp",
+    "sstream": "cpp",
+    "stdexcept": "cpp",
+    "stop_token": "cpp",
+    "streambuf": "cpp",
+    "thread": "cpp",
+    "cinttypes": "cpp",
+    "typeindex": "cpp",
+    "typeinfo": "cpp",
+    "variant": "cpp"
+  }
 }

build.gradle

@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)

src/main/cpp/IO/ClawIO.cpp

@@ -0,0 +1,29 @@
+
+#include "io/ClawIO.h"
+
+void ClawIO::Periodic() {
+    // Process all the inputs and outputs to/from high level software.
+    VOKC_CALL(ProcessIO());
+}
+
+void ClawIO::SimulationPeriodic() {
+    // SimulationPeriodic
+}
+
+bool ClawIO::ProcessIO() {
+    OKC_CHECK(sw_interface_ != nullptr);
+    OKC_CHECK(hw_interface_ != nullptr);
+
+    // Set the software outputs
+    // If the encoder should be reset, reset it
+    if (sw_interface_->reset_claw_open_and_close) {
+         hw_interface_->claw_open_and_close_encoder->SetPosition (0);
+        // Lower the encoder reset flag
+        sw_interface_->reset_claw_open_and_close = false;
+    }
+    hw_interface_->claw_open_and_close_motor->Set (sw_interface_->claw_open_and_close_power);
+    sw_interface_->encoder_reading = hw_interface_->claw_open_and_close_encoder->GetPosition();
+
+    return true;
+}
+

src/main/cpp/Paramaters.cpp

@@ -0,0 +1,33 @@
+#include "Parameters.h"
+
+namespace RobotParams {
+    toml::table parameters;
+
+    // Save parameter file location.
+    std::string param_file =
+        frc::filesystem::GetDeployDirectory() + "/parameters.toml";
+
+    bool LoadParameters(const std::string &path) {
+        // Load the parameters from a file.
+        try {
+            parameters = toml::parse_file(path.c_str());
+        } catch (const toml::parse_error &err) {
+            std::cerr << "Parsing failed:\n" << err << "\n";
+            return false;
+        }
+
+        return true;
+    }
+
+    bool SaveParameters(const std::string &path) {
+        // Output the parameters table to the file.
+        std::ofstream file;
+        file.open(path, std::ofstream::out | std::ofstream::trunc);
+        file << parameters << std::flush;
+        file.close();
+
+        return true;
+    }
+
+} // namespace RobotParams
+

src/main/cpp/Utils.cpp

@@ -0,0 +1,21 @@
+#include "Utils.h"
+
+namespace TeamOKC {
+    // TODO: make this a templated function.
+    bool Clamp(const double &lower, const double &upper, double *value) {
+        OKC_CHECK(value != nullptr);
+
+        // Ensure value is not lower than the minimum or higher than the
+        // maximum. If it is out of the bounds, set it to the boundary value it
+        // violates.
+        if (*value < lower) {
+            *value = lower;
+        } else if (*value > upper) {
+            *value = upper;
+        } else {
+            // Nothing to do here, value is within the range.
+        }
+
+        return true;
+    }
+} // namespace TeamOKC

src/main/cpp/subsystems/claw.cpp

@@ -0,0 +1,21 @@
+#include "subsystems/Claw.h"
+
+bool Claw::Init(){
+    return true;
+}
+
+bool Claw::ResetPositionEncoder(){
+    return true;
+}
+bool Claw::ResetPositionPID(){
+    return true;
+}
+bool Claw::SetPosition(){
+    // SetPosition (30); //cone
+    // SetPosition (0); //close
+    // SetPosition (50);//cube
+    return true;
+}
+bool Claw::Reset(){
+    return true;
+}

src/main/include/Parameters.h

@@ -0,0 +1,28 @@
+
+#pragma once
+
+#include <fstream>
+#include <string.h>
+
+#include <frc/Filesystem.h>
+
+#include "Utils.h"
+#include "third_party/toml.hpp"
+
+namespace RobotParams {
+    extern toml::table parameters;
+    extern std::string param_file;
+
+    bool LoadParameters(const std::string &path);
+    bool SaveParameters(const std::string &path);
+
+    template <typename T>
+    T GetParam(const std::string_view &path, T default_val) {
+        return parameters.at_path(path).value_or(default_val);
+    }
+
+    template <typename T> bool SetParam(const std::string_view &path, T value) {
+        parameters.insert_or_assign(path, value);
+        return true;
+    }
+} // namespace RobotParams

src/main/include/RobotContainer.h

@@ -6,7 +6,8 @@
 
 #include <frc2/command/Command.h>
 
-
+#include "frc/Joystick.h"
+#include "frc2/command/button/JoystickButton.h"
 
 /**
  * This class is where the bulk of the robot should be declared.  Since
@@ -26,4 +27,47 @@ class RobotContainer {
 
 
   void ConfigureButtonBindings();
+
+      std::shared_ptr<frc::Joystick> gamepad1_;
+    std::shared_ptr<frc::Joystick> gamepad2_;
+
+    std::shared_ptr<frc2::JoystickButton> driver_a_button_;
+    std::shared_ptr<frc2::JoystickButton> driver_b_button_;
+    std::shared_ptr<frc2::JoystickButton> driver_back_button_;
+    std::shared_ptr<frc2::JoystickButton> driver_left_bumper_;
+    std::shared_ptr<frc2::JoystickButton> driver_right_bumper_;
+
+    std::shared_ptr<frc2::JoystickButton> manip_a_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_b_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_left_stick_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_right_stick_button;
+
+    std::shared_ptr<frc2::JoystickButton> manip_trigger_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_left_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_bottom_right_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_top_right_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_botton_left_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_top_left_button_;
+
+    std::shared_ptr<frc2::JoystickButton> manip_seven_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_eight_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_nine_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_ten_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_eleven_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_twelve_button_;
+
+    std::shared_ptr<frc2::JoystickButton> manip_A_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_B_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_X_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_Y_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_RB_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_LB_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_RT_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_LT_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_back_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_start_button_;
+    std::shared_ptr<frc2::JoystickButton> manip_mode_button_;
+
 };
+
+

src/main/include/Utils.h

@@ -0,0 +1,61 @@
+
+#pragma once
+
+#include <iostream>
+
+// Despite what VS Code highlighting might suggest, this uses __FUNCTION__ when
+// compiliing using Visual Studio on Windows, and otherwise uses
+// __PRETTY_FUNCTION__
+#if !defined(__PRETTY_FUNCTION__) && !defined(__GNUC__)
+#define __SHOW_LINE_INFO__ __FUNCTION__
+#else
+#define __SHOW_LINE_INFO__ __PRETTY_FUNCTION__
+#endif
+
+#define OKC_CALL(res)                                                          \
+    if (!(res)) {                                                              \
+        std::cerr << "OKC_CHECK FAIL [" << __FILE__ << ":" << __LINE__         \
+                  << "] - " << __SHOW_LINE_INFO__ << std::endl;                \
+        return false;                                                          \
+    }
+
+#define OKC_CHECK(check)                                                       \
+    if (!(check)) {                                                            \
+        std::cerr << "OKC_CHECK FAIL [" << __FILE__ << ":" << __LINE__         \
+                  << "] - " << __SHOW_LINE_INFO__ << std::endl;                \
+        return false;                                                          \
+    }
+
+#define OKC_CHECK_MSG(check, msg)                                              \
+    if (!(check)) {                                                            \
+        std::cerr << "OKC_CHECK FAIL [" << __FILE__ << ":" << __LINE__         \
+                  << "] - " << __SHOW_LINE_INFO__ << ": " << msg << std::endl; \
+        return false;                                                          \
+    }
+
+#define VOKC_CALL(res)                                                         \
+    if (!(res)) {                                                              \
+        std::cerr << "OKC_CHECK FAIL [" << __FILE__ << ":" << __LINE__         \
+                  << "] - " << __SHOW_LINE_INFO__ << std::endl;                \
+        return;                                                                \
+    }
+
+#define VOKC_CHECK(check)                                                      \
+    if (!(check)) {                                                            \
+        std::cerr << "OKC_CHECK FAIL [" << __FILE__ << ":" << __LINE__         \
+                  << "] - " << __SHOW_LINE_INFO__ << std::endl;                \
+        return;                                                                \
+    }
+
+#define VOKC_CHECK_MSG(check, msg)                                             \
+    if (!(check)) {                                                            \
+        std::cerr << "OKC_CHECK FAIL [" << __FILE__ << ":" << __LINE__         \
+                  << "] - " << __SHOW_LINE_INFO__ << ": " << msg << std::endl; \
+        return;                                                                \
+    }
+
+namespace TeamOKC {
+
+    bool Clamp(const double &lower, const double &upper, double *value);
+
+}