Arm improvement and tuning (#69)

* code for new intake with wrist (#58)

* 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

* feat/field-oriented-scoring (#62)

* field oreinted scoring written, builds, untested

* field-oriented scoring tested, works

* left and right bumpers for pickup positions so we can pickup on both sides of the robot

* fix arm offset

* 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

* fix intake wrist tilt encoder port

* log the intake tilt output, setpoint, and sensor reading

* field-oriented intake tilting commands

* wrap the tilt encoder because the zero-point is in a really annoying spot and fix the setpoints

* log the intake wrist values, and show them on shuffleboard,

* if arm is close to 0 and trying to go to 0 then prevent it from oscillating

* added wrist setpoint to shuffleboard

* made intake faster

* started work on a command to pause auton execution for the arm

* wrist setpoint on shuffleboard

* fix extend thresholds/setpoints because we replaced the gearboxes

* intake wrist tilt tuning and fixing

* code no workey tilt then move claw arm thingy

* Improved and Tuned Arm and Added ff to arm control

* fix PR comments

---------

Co-authored-by: Daniel <56051347+danielbrownmsm@users.noreply.github.com>
Co-authored-by: danielbrownmsm <daniel.brown@msmrockets.org>

Author: deadlyvortex

build.gradle

@@ -36,7 +36,7 @@ deploy {
 def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
 
 // Set this to true to enable desktop support.
-def includeDesktopSupport = false
+def includeDesktopSupport = true
 
 // Set to true to run simulation in debug mode
 wpi.cpp.debugSimulation = false

src/main/cpp/RobotContainer.cpp

@@ -70,6 +70,7 @@ bool RobotContainer::ConfigureButtonBindings() {
     // second driver controls
     manip_right_bumper_button_->WhenPressed(*pickup_command_);
     manip_left_bumper_button_->WhenPressed(*pickup_reverse_command_);
+
     manip_x_button_->WhenPressed(*carry_command_);
 
     manip_b_button_->WhenPressed(*score_mid_command_);

src/main/cpp/Utils.cpp

@@ -32,5 +32,16 @@ namespace TeamOKC {
     double Radians(double degrees) {
         return degrees * M_PI / 180.0;
     }
+    double sign(double value) {
+        if (value>0) {
+            return 1;
+        }
+        if (value<0) {
+            return -1;
+        }
+        if (value==0) {
+            return 0;
+        }
+    }
 } // namespace TeamOKC
 

src/main/cpp/subsystems/Arm.cpp

@@ -1,6 +1,8 @@
 #include "subsystems/Arm.h"
 #include "Parameters.h"
 #include "ui/UserInterface.h"
+#include "Utils.h"
+
 
 bool Arm::Init() {
     // initializing the arm
@@ -10,8 +12,10 @@ bool Arm::Init() {
     double arm_kP = RobotParams::GetParam("arm.lift_pid.kP", 0.005);
     double arm_kI = RobotParams::GetParam("arm.lift_pid.kI", 0.0);
     double arm_kD = RobotParams::GetParam("arm.lift_pid.kD", 0.0);
+    double arm_kF = RobotParams::GetParam("arm.lift_pid.kF", 0.05);
     arm_pid_ = std::make_shared<frc::PIDController>(arm_kP, arm_kI, arm_kD);
     arm_pid_->SetTolerance(2, 2);
+    arm_kF_ = arm_kF;
 
     double extend_kP = RobotParams::GetParam("arm.extend_pid.kP", 0.005);
     double extend_kI = RobotParams::GetParam("arm.extend_pid.kI", 0.0);
@@ -42,6 +46,7 @@ bool Arm::Init() {
     return true;
 }
 
+
 bool Arm::SetControlMode(const ControlMode &mode){
     mode_= mode;
 
@@ -146,8 +151,12 @@ bool Arm::TestControl() {
     this->arm_pid_->SetSetpoint(this->desired_state_.rotation);
     this->inches_pid_->SetSetpoint(this->desired_state_.extension);
 
+    // calculate feedforward
+    // take the sign of the desired state, multiply it by the feedforward gain times the desired rotation squared. this looks like output = signum(setpoint) * kF * setpoint^2
+    double ff = TeamOKC::sign(this->desired_state_.rotation) * arm_kF_ * this->desired_state_.rotation * this->desired_state_.rotation;
+    
     // set output
-    this->interface_->arm_lift_power = this->arm_pid_->Calculate(this->state_.rotation);
+    this->interface_->arm_lift_power = this->arm_pid_->Calculate(this->state_.rotation) + ff;
     this->interface_->arm_extend_power = this->inches_pid_->Calculate(this->state_.extension);
 
     return true;

src/main/deploy/parameters.toml

@@ -88,7 +88,7 @@ arm_setpoint = 94.0
 intake_setpoint = -40.0
 
 [arm.negative_pickup]
-extend_setpoint = 50.0
+extend_setpoint = 20.0
 arm_setpoint = -25.0
 intake_setpoint = -70.0
 

src/main/include/Utils.h

@@ -62,6 +62,7 @@ namespace TeamOKC {
 
     bool WrapAngle(double *angle);
     double Radians(double degrees);
+    double sign(double value);
 
     typedef struct arm_state_t {
         double extension;

src/main/include/subsystems/Arm.h

@@ -4,6 +4,7 @@
 #include <frc2/command/SubsystemBase.h>
 #include <io/ArmIO.h>
 #include <memory>
+#include "Utils.h"
 
 #include "Logging.h"
 #include "wpi/DataLog.h"
@@ -56,6 +57,7 @@ class Arm : public frc2::SubsystemBase {
     // PID controllers
     std::shared_ptr<frc::PIDController> arm_pid_;
     std::shared_ptr<frc::PIDController> inches_pid_;
+    double arm_kF_;
 
     // controls stuff (I'd call it a state machine but it isn't really a state machine)
     ControlMode mode_;