Feat/wrist intake (#63)

* code for new intake with wrist
still need to write commands for it though

* actually create hardware objects for the new intake

* update values from hardware in the software interface

* wrote commands to move the wrist to a certain position, either just straight to a position or incrementing it

* bind some of the wrist commands we need

* wrist tilt encoder on shuffleboard

* clean up the intake commands and add some more OKC_CALL stuff

* add an offset to the tilt encoder

* fix intake parameters

* more okc checks

Author: danielbrownmsm

src/main/cpp/RobotContainer.cpp

@@ -71,6 +71,9 @@ bool RobotContainer::ConfigureButtonBindings() {
 
     manip_b_button_->WhenPressed(*arm_score_mid_command_);
     manip_y_button_->WhenPressed(*arm_score_high_command_);
+
+    manip_left_stick_button_->WhenHeld(*inc_wrist_tilt_command_);
+    manip_right_stick_button_->WhenHeld(*dec_wrist_tilt_command_);
 
     WPI_UNIGNORE_DEPRECATED
 
@@ -343,6 +346,9 @@ bool RobotContainer::InitCommands() {
     intake_command = std::make_shared<IntakeCommand>(intake_, 0.3);
     other_intake_command = std::make_shared<IntakeCommand>(intake_, -0.3);
     stop_intake_command = std::make_shared<IntakeCommand>(intake_, -0.01);
+
+    inc_wrist_tilt_command_ = std::make_shared<IncrementIntakePositionCommand>(intake_, 1);
+    dec_wrist_tilt_command_ = std::make_shared<IncrementIntakePositionCommand>(intake_, -1);
 
     return true;
 }

src/main/cpp/commands/intake/IncrementIntakePositionCommand.cpp

@@ -0,0 +1,33 @@
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "Utils.h"
+#include "subsystems/Intake.h"
+
+#include "commands/intake/IncrementIntakePositionCommand.h"
+
+IncrementIntakePositionCommand::IncrementIntakePositionCommand(std::shared_ptr<Intake> intake, double angle) {
+    // Set everything.
+    intake_ = intake;
+    angle_ = angle;
+
+    if (intake_ != nullptr) {
+        this->AddRequirements(intake_.get());
+    }
+}
+
+void IncrementIntakePositionCommand::Initialize() {
+    VOKC_CHECK(intake_ != nullptr);
+    VOKC_CALL(intake_->SetControlMode(Auto));
+}
+
+
+void IncrementIntakePositionCommand::Execute() {
+    VOKC_CHECK(intake_ != nullptr);
+    VOKC_CALL(intake_->IncrementIntakeTilt(angle_));
+}
+
+bool IncrementIntakePositionCommand::IsFinished() {
+    // Always end this command.
+    return true;
+}

src/main/cpp/commands/intake/IntakeCommand.cpp

@@ -13,20 +13,19 @@ IntakeCommand::IntakeCommand(std::shared_ptr<Intake> intake,
     power_ = power;
 
     if (intake_ != nullptr) {
-        this->AddRequirements(intake_.get());
-    
-        intake_->SetControlMode(Manual);
+        this->AddRequirements(intake_.get());   
     }
 }
 
-
+void IntakeCommand::Initialize() {
+    VOKC_CALL(intake_ != nullptr);
+    VOKC_CALL(intake_->SetControlMode(Manual));
+}
 
 
 void IntakeCommand::Execute() {
     VOKC_CHECK(intake_ != nullptr);
     VOKC_CALL(intake_->SetIntakePower(power_))
-
-
 }
 
 bool IntakeCommand::IsFinished() {

src/main/cpp/commands/intake/IntakePositionCommand.cpp

@@ -0,0 +1,33 @@
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "Utils.h"
+#include "subsystems/Intake.h"
+
+#include "commands/intake/IntakePositionCommand.h"
+
+IntakePositionCommand::IntakePositionCommand(std::shared_ptr<Intake> intake, double angle) {
+    // Set everything.
+    intake_ = intake;
+    angle_ = angle;
+
+    if (intake_ != nullptr) {
+        this->AddRequirements(intake_.get());
+    }
+}
+
+void IntakePositionCommand::Initialize() {
+    VOKC_CHECK(intake_ != nullptr)
+    VOKC_CALL(intake_->SetControlMode(Auto));
+}
+
+
+void IntakePositionCommand::Execute() {
+    VOKC_CHECK(intake_ != nullptr);
+    VOKC_CALL(intake_->SetIntakeTilt(angle_));
+}
+
+bool IntakePositionCommand::IsFinished() {
+    // Always end this command.
+    return true;
+}

src/main/cpp/io/IntakeIO.cpp

@@ -2,7 +2,9 @@
 #include "Parameters.h"
 
 bool IntakeIO::Init() {
-   return true;
+    offset_ = RobotParams::GetParam("intake.tilt_offset", 0.0);
+
+    return true;
 }
 
 void IntakeIO::Periodic() {
@@ -43,7 +45,7 @@ bool IntakeIO::ProcessIO() {
 
     // wrist encoder
     OKC_CHECK(hw_interface_->wrist_encoder != nullptr);
-    sw_interface_->tilt_encoder = hw_interface_->wrist_encoder->GetAbsolutePosition(); // TODO parameterize and offset and stuff
+    sw_interface_->tilt_encoder = hw_interface_->wrist_encoder->GetAbsolutePosition() + offset_;
 
     // === outputs ===
     hw_interface_->intake_motor->Set(sw_interface_->intake_power);

src/main/cpp/subsystems/Intake.cpp

@@ -7,9 +7,9 @@ bool Intake::Init() {
     OKC_CHECK(this->interface_ != nullptr);
 
     // the PID controller for the wrist
-    double wrist_kP = RobotParams::GetParam("intale.wrist_pid.kP", 0.005);
-    double wrist_kI = RobotParams::GetParam("intale.wrist_pid.kI", 0.0);
-    double wrist_kD = RobotParams::GetParam("intale.wrist_pid.kD", 0.0);
+    double wrist_kP = RobotParams::GetParam("intake.wrist_pid.kP", 0.005);
+    double wrist_kI = RobotParams::GetParam("intake.wrist_pid.kI", 0.0);
+    double wrist_kD = RobotParams::GetParam("intake.wrist_pid.kD", 0.0);
     wrist_pid_ = std::make_shared<frc::PIDController>(wrist_kP, wrist_kI, wrist_kD);
 
     // wrist_pid_->SetTolerance(1, 1);
@@ -42,6 +42,14 @@ bool Intake::SetIntakeTilt(double degrees) {
     return true;
 }
 
+bool Intake::IncrementIntakeTilt(double degrees) {
+    OKC_CHECK(this->wrist_pid_ != nullptr);
+
+    this->wrist_pid_->SetSetpoint(this->wrist_pid_->GetSetpoint() + degrees);
+
+    return true;
+}
+
 bool Intake::SetControlMode(const ControlMode &mode){
     mode_= mode;
 
@@ -61,6 +69,7 @@ bool Intake::AutoControl() {
     OKC_CHECK(this->wrist_pid_ != nullptr);
 
     interface_->tilt_power = this->wrist_pid_->Calculate(interface_->tilt_encoder);
+    interface_->intake_power = intake_power_;
 
     return true;
 }
@@ -80,6 +89,8 @@ void Intake::Periodic() {
         default:
             VOKC_CHECK_MSG(false, "unhandled intake enum");
     }
+
+    VOKC_CALL(IntakeUI::nt_tilt->SetDouble(this->interface_->tilt_encoder));
 }
 
 

src/main/cpp/ui/UserInterface.cpp

@@ -65,4 +65,10 @@ namespace ClawUI {
     frc::ShuffleboardTab &nt_tab = frc::Shuffleboard::GetTab("claw");
 
     nt::GenericEntry *const nt_claw_encoder = nt_tab.Add("claw encoder", 0.0).GetEntry();
+}
+
+namespace IntakeUI {
+    frc::ShuffleboardTab &nt_tab = frc::Shuffleboard::GetTab("claw");
+
+    nt::GenericEntry *const nt_tilt = nt_tab.Add("wrist encoder", 0.0).GetEntry();
 }

src/main/deploy/parameters.toml

@@ -107,3 +107,6 @@ cone = 5
 kP = 0.01
 kI = 0
 kD = 0
+
+[intake]
+tilt_offset = 0.0

src/main/include/RobotContainer.h

@@ -48,6 +48,8 @@
 
 //intake
 #include "commands/intake/IntakeCommand.h"
+#include "commands/intake/IntakePositionCommand.h"
+#include "commands/intake/IncrementIntakePositionCommand.h"
 
 // misc
 #include <frc2/command/Command.h>
@@ -182,4 +184,7 @@ class RobotContainer {
     std::shared_ptr<IntakeCommand> intake_command;
     std::shared_ptr<IntakeCommand> other_intake_command;
     std::shared_ptr<IntakeCommand> stop_intake_command;
+
+    std::shared_ptr<IncrementIntakePositionCommand> inc_wrist_tilt_command_;
+    std::shared_ptr<IncrementIntakePositionCommand> dec_wrist_tilt_command_;
 };

src/main/include/commands/intake/IncrementIntakePositionCommand.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "Utils.h"
+#include "subsystems/Intake.h"
+
+/**
+ *
+ */
+class IncrementIntakePositionCommand
+    : public frc2::CommandHelper<frc2::CommandBase, IncrementIntakePositionCommand> {
+public:
+    /**
+     * Creates a new IncrementArmExtendCommand.
+     *
+     * @param arm The subsystem used by this command.
+     */
+    explicit IncrementIntakePositionCommand(std::shared_ptr<Intake> intake, double angle);
+
+    void Execute() override;
+    void Initialize() override;
+    bool IsFinished() override;
+
+private:
+    std::shared_ptr<Intake> intake_;
+    double angle_;
+};