From b640df00391508f4fa1f26e26a3a209fcb73ebb3 Mon Sep 17 00:00:00 2001 From: Daniel <56051347+danielbrownmsm@users.noreply.github.com> Date: Thu, 23 Mar 2023 17:46:44 -0400 Subject: [PATCH 01/19] 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 --- src/main/cpp/RobotContainer.cpp | 2 ++ src/main/cpp/hardware/Hardware.cpp | 4 ++- src/main/cpp/io/IntakeIO.cpp | 19 ++++++---- src/main/cpp/subsystems/Intake.cpp | 52 +++++++++++++++++---------- src/main/include/hardware/Actuators.h | 4 ++- src/main/include/hardware/Sensors.h | 2 ++ src/main/include/io/IntakeIO.h | 6 ++++ src/main/include/subsystems/Intake.h | 26 ++++++-------- 8 files changed, 74 insertions(+), 41 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 89048ea..3d360c5 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -125,6 +125,7 @@ bool RobotContainer::InitActuators(Actuators *actuators_interface) { actuators_interface->arm_extend_motor->SetIdleMode(BRAKE); actuators_interface->intake_motor = std::make_unique(INTAKE_MOTOR, BRUSHLESS); + actuators_interface->wrist_motor = std::make_unique(WRIST_MOTOR, BRUSHLESS); OKC_CHECK(actuators_interface->intake_motor != nullptr); @@ -195,6 +196,7 @@ bool RobotContainer::InitSensors(const Actuators &actuators, OKC_CHECK(sensor_interface->arm_lift_encoder != nullptr); sensor_interface->intake_encoder = std::make_unique(actuators.intake_motor->GetEncoder()); + sensor_interface->wrist_encoder = std::make_unique(WRIST_ABS_ENCODER); return true; } diff --git a/src/main/cpp/hardware/Hardware.cpp b/src/main/cpp/hardware/Hardware.cpp index 97a7a37..a96a64e 100644 --- a/src/main/cpp/hardware/Hardware.cpp +++ b/src/main/cpp/hardware/Hardware.cpp @@ -119,7 +119,9 @@ bool SetupIntakeInterface(std::unique_ptr &hardware, std::shared_ptrintake_motor.get(), - sensors->intake_encoder.get() + sensors->intake_encoder.get(), + actuators->wrist_motor.get(), + sensors->wrist_encoder.get() }; interface = std::make_shared(intake_interface); diff --git a/src/main/cpp/io/IntakeIO.cpp b/src/main/cpp/io/IntakeIO.cpp index b54d694..37875f2 100644 --- a/src/main/cpp/io/IntakeIO.cpp +++ b/src/main/cpp/io/IntakeIO.cpp @@ -36,22 +36,30 @@ bool IntakeIO::ProcessIO() { sw_interface_->reset_encoders = false; } - // intake position encoder + // === inputs === + // intake encoder OKC_CHECK(hw_interface_->intake_motor != nullptr); - - sw_interface_->intake_encoder = hw_interface_->intake_encoder->GetPosition(); + // wrist encoder + OKC_CHECK(hw_interface_->wrist_encoder != nullptr); + sw_interface_->tilt_encoder = hw_interface_->wrist_encoder->GetAbsolutePosition(); // TODO parameterize and offset and stuff + + // === outputs === hw_interface_->intake_motor->Set(sw_interface_->intake_power); + hw_interface_->wrist_motor->Set(sw_interface_->tilt_power); -return true; + return true; } + bool IntakeIO::UpdateIntakeConfig(IntakeConfig &config) { OKC_CHECK(hw_interface_ != nullptr); + + // TODO + return true; } - // Get the configuration //HACK: this is a hack bool IntakeIO::ResetEncoders() { @@ -59,7 +67,6 @@ bool IntakeIO::ResetEncoders() { hw_interface_->intake_motor->GetEncoder().SetPosition(0.0); - return true; } diff --git a/src/main/cpp/subsystems/Intake.cpp b/src/main/cpp/subsystems/Intake.cpp index 4fdbaf7..5221927 100644 --- a/src/main/cpp/subsystems/Intake.cpp +++ b/src/main/cpp/subsystems/Intake.cpp @@ -4,30 +4,46 @@ //pulls PID values from the parameters.toml file bool Intake::Init() { + OKC_CHECK(this->interface_ != nullptr); - intake_pid_ = std::make_shared(0, 0, 0); - -this->intake_pid_->SetSetpoint(0); + // 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); + wrist_pid_ = std::make_shared(wrist_kP, wrist_kI, wrist_kD); + + // wrist_pid_->SetTolerance(1, 1); + //TODO verify that this is a good idea + this->wrist_pid_->SetSetpoint(0.0); -return true; + return true; } -bool Intake::SetControlMode(const ControlMode &mode){ - mode_= mode; +bool Intake::Reset() { + OKC_CHECK(this->interface_ != nullptr); + + this->wrist_pid_->Reset(); return true; +} + +bool Intake::SetIntakePower(double power) { + intake_power_ = power; + return true; } -bool Intake::SetTurn(double degrees) { + +bool Intake::SetIntakeTilt(double degrees) { + OKC_CHECK(this->wrist_pid_ != nullptr); + + this->wrist_pid_->SetSetpoint(degrees); - OKC_CHECK(this->intake_pid_ != nullptr); - return true; + return true; } - -bool Intake::SetIntakePower(double power) { - intake_power_ = power; +bool Intake::SetControlMode(const ControlMode &mode){ + mode_= mode; return true; } @@ -37,15 +53,16 @@ bool Intake::ManualControl() { interface_->intake_power = intake_power_; - return true; } - bool Intake::AutoControl() { OKC_CHECK(interface_ != nullptr); - OKC_CHECK(this->intake_pid_ != nullptr); - return true; + OKC_CHECK(this->wrist_pid_ != nullptr); + + interface_->tilt_power = this->wrist_pid_->Calculate(interface_->tilt_encoder); + + return true; } void Intake::SimulationPeriodic() { @@ -53,7 +70,6 @@ void Intake::SimulationPeriodic() { } void Intake::Periodic() { - switch (mode_) { case Manual: VOKC_CALL(this->ManualControl()); @@ -62,7 +78,7 @@ void Intake::Periodic() { VOKC_CALL(this->AutoControl()); break; default: - VOKC_CHECK_MSG(false, "Unhandled enum"); + VOKC_CHECK_MSG(false, "unhandled intake enum"); } } diff --git a/src/main/include/hardware/Actuators.h b/src/main/include/hardware/Actuators.h index 578ce8e..5042f50 100644 --- a/src/main/include/hardware/Actuators.h +++ b/src/main/include/hardware/Actuators.h @@ -30,6 +30,7 @@ #define CLAW_MOTOR 12 #define INTAKE_MOTOR 12 +#define WRIST_MOTOR 13 typedef struct actuators_t { @@ -53,6 +54,7 @@ typedef struct actuators_t { // Claw things std::unique_ptr claw_motor; - //intake motor + //intake motors std::unique_ptr intake_motor; + std::unique_ptr wrist_motor; } Actuators; \ No newline at end of file diff --git a/src/main/include/hardware/Sensors.h b/src/main/include/hardware/Sensors.h index d5e411c..4b3ea3a 100644 --- a/src/main/include/hardware/Sensors.h +++ b/src/main/include/hardware/Sensors.h @@ -18,6 +18,7 @@ // DIO ports (on the RIO) #define EXTEND_LIMIT_SWITCH 9 #define ARM_ABS_ENCODER 1 +#define WRIST_ABS_ENCODER 4 typedef struct sensors_t { // navX IMU @@ -53,4 +54,5 @@ typedef struct sensors_t { //intake sensord std::unique_ptr intake_encoder; + std::unique_ptr wrist_encoder; } Sensors; diff --git a/src/main/include/io/IntakeIO.h b/src/main/include/io/IntakeIO.h index 9e64e99..576715c 100644 --- a/src/main/include/io/IntakeIO.h +++ b/src/main/include/io/IntakeIO.h @@ -19,14 +19,20 @@ typedef struct intake_config_t { typedef struct intake_hardware_interface_t { rev::CANSparkMax *const intake_motor; rev::RelativeEncoder *const intake_encoder; + + rev::CANSparkMax *const wrist_motor; + frc::DutyCycleEncoder *const wrist_encoder; // (rev through-bore encoder in the absolute position mode) } IntakeHardwareInterface; typedef struct intake_software_interface_t { // actuator outputs double intake_power; + double tilt_power; + // sensor inputs double intake_encoder; + double tilt_encoder; // config diff --git a/src/main/include/subsystems/Intake.h b/src/main/include/subsystems/Intake.h index dfae23a..fbc8858 100644 --- a/src/main/include/subsystems/Intake.h +++ b/src/main/include/subsystems/Intake.h @@ -14,30 +14,26 @@ class Intake : public frc2::SubsystemBase { Intake(IntakeSoftwareInterface *interface) : interface_(interface) {} ~Intake() {} - bool SetControlMode(const ControlMode &mode); bool Init(); - bool SetTurn(double degrees); - bool ManualControl(); - bool AutoControl(); + bool SetIntakePower(double power); - void Periodic() override; - void SimulationPeriodic() override; - - + bool SetIntakeTilt(double tilt); + bool SetControlMode(const ControlMode &mode); + bool ManualControl(); + bool AutoControl(); - - + bool Reset(); + void Periodic() override; + void SimulationPeriodic() override; private: IntakeSoftwareInterface *const interface_; - std::shared_ptr intake_pid_; + + std::shared_ptr wrist_pid_; - double preset_; double intake_power_; - - ControlMode mode_; - + ControlMode mode_; }; From 3dd924a95c5e86f3ff5fe0c41f2c69db0632830f Mon Sep 17 00:00:00 2001 From: Daniel <56051347+danielbrownmsm@users.noreply.github.com> Date: Thu, 23 Mar 2023 20:14:19 -0400 Subject: [PATCH 02/19] 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 --- src/main/cpp/RobotContainer.cpp | 28 ++++++---- .../commands/arm/ArmFieldOrientedCommand.cpp | 55 +++++++++++++++++++ src/main/deploy/parameters.toml | 2 +- src/main/include/RobotContainer.h | 10 ++-- .../commands/arm/ArmFieldOrientedCommand.h | 29 ++++++++++ 5 files changed, 106 insertions(+), 18 deletions(-) create mode 100644 src/main/cpp/commands/arm/ArmFieldOrientedCommand.cpp create mode 100644 src/main/include/commands/arm/ArmFieldOrientedCommand.h diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 3d360c5..ed09da0 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -65,15 +65,12 @@ bool RobotContainer::ConfigureButtonBindings() { // driver_b_button_->WhileActiveContinous(*extendArmCommand); // second driver controls + manip_right_bumper_button_->WhenPressed(*arm_pickup_reverse_command_); + manip_left_bumper_button_->WhenPressed(*arm_pickup_command_); manip_x_button_->WhenPressed(*arm_carry_command_); - manip_a_button_->WhenPressed(*arm_pickup_command_); + manip_b_button_->WhenPressed(*arm_score_mid_command_); manip_y_button_->WhenPressed(*arm_score_high_command_); - manip_left_bumper_button_->WhenPressed(*arm_short_carry_command_); - - manip_right_bumper_button_->WhenPressed(*slow_swerve_teleop_command_).WhenReleased(*slow_swerve_teleop_command_); - - manip_start_button_->WhenPressed(*arm_dpad_set_state_command_); WPI_UNIGNORE_DEPRECATED @@ -306,6 +303,16 @@ bool RobotContainer::InitCommands() { double score_high_rotation_ = RobotParams::GetParam("arm.score_high.arm_setpoint", 0.0); double score_high_extension_ = RobotParams::GetParam("arm.score_high.extend_setpoint", 0.0); + double negative_pickup_rotation_ = RobotParams::GetParam("arm.negative_pickup.arm_setpoint", 0.0); + double negative_pickup_extension_ = RobotParams::GetParam("arm.negative_pickup.extend_setpoint", 1.0); + + double negative_score_mid_rotation_ = RobotParams::GetParam("arm.negative_score_medium.arm_setpoint", 0.0); + double negative_score_mid_extension_ = RobotParams::GetParam("arm.negative_score_medium.extend_setpoint", 1.0); + + double negative_score_high_rotation_ = RobotParams::GetParam("arm.negative_score_high.arm_setpoint", 0.0); + double negative_score_high_extension_ = RobotParams::GetParam("arm.negative_score_high.extend_setpoint", 1.0); + + // autons score_preload_backup_auto_ = std::make_shared(swerve_drive_, arm_, intake_); score_preload_auto_ = std::make_shared(arm_, intake_); @@ -325,13 +332,12 @@ bool RobotContainer::InitCommands() { lowerArmCommand = std::make_shared(arm_, -0.5); // arm commands + arm_carry_command_ = std::make_shared(arm_, TeamOKC::ArmState(1, 0)); arm_pickup_command_ = std::make_shared(arm_, TeamOKC::ArmState(pickup_extension_, pickup_rotation_)); - arm_score_mid_command_ = std::make_shared(arm_, TeamOKC::ArmState(score_mid_extension_, score_mid_rotation_)); - arm_score_high_command_ = std::make_shared(arm_, TeamOKC::ArmState(score_high_extension_, score_high_rotation_)); - arm_carry_command_ = std::make_shared(arm_, TeamOKC::ArmState(1, 0)); // hold the arm inside the robot when driving - arm_short_carry_command_ = std::make_shared(arm_, TeamOKC::ArmState(2, pickup_rotation_)); // just bring teh arm a little in whenever we're moving in the community + arm_pickup_reverse_command_ = std::make_shared(arm_, TeamOKC::ArmState(negative_pickup_extension_, negative_pickup_rotation_)); - arm_dpad_set_state_command_ = std::make_shared(arm_, gamepad2_); + arm_score_mid_command_ = std::make_shared(arm_, swerve_drive_, TeamOKC::ArmState(score_mid_extension_, score_mid_rotation_), TeamOKC::ArmState(negative_score_mid_extension_, negative_score_mid_rotation_)); + arm_score_high_command_ = std::make_shared(arm_, swerve_drive_, TeamOKC::ArmState(score_high_extension_, score_high_rotation_), TeamOKC::ArmState(negative_score_high_extension_, negative_score_high_rotation_)); // intake commands intake_command = std::make_shared(intake_, 0.3); diff --git a/src/main/cpp/commands/arm/ArmFieldOrientedCommand.cpp b/src/main/cpp/commands/arm/ArmFieldOrientedCommand.cpp new file mode 100644 index 0000000..5369f89 --- /dev/null +++ b/src/main/cpp/commands/arm/ArmFieldOrientedCommand.cpp @@ -0,0 +1,55 @@ +#include +#include + +#include "Utils.h" +#include "subsystems/Arm.h" + +#include "commands/arm/ArmFieldOrientedCommand.h" + + + +ArmFieldOrientedCommand::ArmFieldOrientedCommand(std::shared_ptr arm, std::shared_ptr swerve, TeamOKC::ArmState state, TeamOKC::ArmState reverse_state) { + // Set everything. + arm_ = arm; + swerve_drive_ = swerve; + state_ = state; + reverse_state_ = reverse_state; + + if (arm_ != nullptr) { + this->AddRequirements(arm_.get()); + } +} + +void ArmFieldOrientedCommand::Initialize() { + VOKC_CHECK(arm_ != nullptr); + + VOKC_CALL(arm_->SetControlMode(Auto)); +} + +void ArmFieldOrientedCommand::Execute() { + VOKC_CHECK(arm_ != nullptr); + + // get the current robot heading + double heading = 0.0; + VOKC_CALL(swerve_drive_->GetHeading(&heading)); + + // convert to [-180, 180] + TeamOKC::WrapAngle(&heading); + + // if the heading is pointing away from the scoring area + if (abs(heading) < 90.0) { + // score on the normal side of the robot + // set the desired state of the arm + VOKC_CALL(arm_->SetDesiredState(state_)); + } else { + // otherwise, score on the other side + // set the desired state of the arm + VOKC_CALL(arm_->SetDesiredState(reverse_state_)); + } + +} + +bool ArmFieldOrientedCommand::IsFinished() { + // always end this command immediately + return true; +} \ No newline at end of file diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index d307cde..6195aae 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -55,7 +55,7 @@ right_front_offset = 10.0 right_back_offset = 153.0 [arm] -offset = -330 +offset = -390 lift_limit = 110 extend_limit = 95 max_output = 1 diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 5292f79..8a5b258 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -42,8 +42,8 @@ // arm #include "commands/arm/IncrementArmPresetPositionCommand.h" #include "commands/arm/IncrementArmExtendCommand.h" +#include "commands/arm/ArmFieldOrientedCommand.h" #include "commands/arm/ArmSetStateCommand.h" -#include "commands/arm/ArmSetStateDpadCommand.h" //intake @@ -174,11 +174,9 @@ class RobotContainer { std::shared_ptr arm_carry_command_; std::shared_ptr arm_pickup_command_; - std::shared_ptr arm_score_mid_command_; - std::shared_ptr arm_score_high_command_; - std::shared_ptr arm_short_carry_command_; - - std::shared_ptr arm_dpad_set_state_command_; + std::shared_ptr arm_pickup_reverse_command_; + std::shared_ptr arm_score_mid_command_; + std::shared_ptr arm_score_high_command_; //intake std::shared_ptr intake_command; diff --git a/src/main/include/commands/arm/ArmFieldOrientedCommand.h b/src/main/include/commands/arm/ArmFieldOrientedCommand.h new file mode 100644 index 0000000..897f4d5 --- /dev/null +++ b/src/main/include/commands/arm/ArmFieldOrientedCommand.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +#include "Utils.h" +#include "subsystems/Arm.h" +#include "subsystems/SwerveDrive.h" + +class ArmFieldOrientedCommand + : public frc2::CommandHelper { +public: + /** + * Creates a new ArmSetStateCommand. + * + * @param subsystem The subsystem used by this command. + */ + explicit ArmFieldOrientedCommand(std::shared_ptr arm, std::shared_ptr swerve, TeamOKC::ArmState state, TeamOKC::ArmState reverse_state); + + void Initialize() override; + void Execute() override; + bool IsFinished() override; + +private: + std::shared_ptr arm_; + std::shared_ptr swerve_drive_; + TeamOKC::ArmState state_; + TeamOKC::ArmState reverse_state_; +}; \ No newline at end of file From 97597712ee491428f7391d3b106855a09192f437 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sat, 25 Mar 2023 13:27:14 -0500 Subject: [PATCH 03/19] fix arm offset --- src/main/deploy/parameters.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index d307cde..be33c07 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -55,7 +55,7 @@ right_front_offset = 10.0 right_back_offset = 153.0 [arm] -offset = -330 +offset = -384 lift_limit = 110 extend_limit = 95 max_output = 1 From 1a284b5c4dd342b5a02f7c46d2988ca52c2826ba Mon Sep 17 00:00:00 2001 From: Daniel <56051347+danielbrownmsm@users.noreply.github.com> Date: Sat, 25 Mar 2023 14:45:04 -0400 Subject: [PATCH 04/19] 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 --- src/main/cpp/RobotContainer.cpp | 6 ++++ .../intake/IncrementIntakePositionCommand.cpp | 33 +++++++++++++++++++ .../cpp/commands/intake/IntakeCommand.cpp | 11 +++---- .../commands/intake/IntakePositionCommand.cpp | 33 +++++++++++++++++++ src/main/cpp/io/IntakeIO.cpp | 6 ++-- src/main/cpp/subsystems/Intake.cpp | 17 ++++++++-- src/main/cpp/ui/UserInterface.cpp | 6 ++++ src/main/deploy/parameters.toml | 3 ++ src/main/include/RobotContainer.h | 5 +++ .../intake/IncrementIntakePositionCommand.h | 29 ++++++++++++++++ .../include/commands/intake/IntakeCommand.h | 1 + .../commands/intake/IntakePositionCommand.h | 29 ++++++++++++++++ src/main/include/io/IntakeIO.h | 1 + src/main/include/subsystems/Intake.h | 2 ++ src/main/include/ui/UserInterface.h | 6 ++++ 15 files changed, 177 insertions(+), 11 deletions(-) create mode 100644 src/main/cpp/commands/intake/IncrementIntakePositionCommand.cpp create mode 100644 src/main/cpp/commands/intake/IntakePositionCommand.cpp create mode 100644 src/main/include/commands/intake/IncrementIntakePositionCommand.h create mode 100644 src/main/include/commands/intake/IntakePositionCommand.h diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index ed09da0..25e446d 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/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(intake_, 0.3); other_intake_command = std::make_shared(intake_, -0.3); stop_intake_command = std::make_shared(intake_, -0.01); + + inc_wrist_tilt_command_ = std::make_shared(intake_, 1); + dec_wrist_tilt_command_ = std::make_shared(intake_, -1); return true; } diff --git a/src/main/cpp/commands/intake/IncrementIntakePositionCommand.cpp b/src/main/cpp/commands/intake/IncrementIntakePositionCommand.cpp new file mode 100644 index 0000000..e57680b --- /dev/null +++ b/src/main/cpp/commands/intake/IncrementIntakePositionCommand.cpp @@ -0,0 +1,33 @@ +#include +#include + +#include "Utils.h" +#include "subsystems/Intake.h" + +#include "commands/intake/IncrementIntakePositionCommand.h" + +IncrementIntakePositionCommand::IncrementIntakePositionCommand(std::shared_ptr 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; +} \ No newline at end of file diff --git a/src/main/cpp/commands/intake/IntakeCommand.cpp b/src/main/cpp/commands/intake/IntakeCommand.cpp index 99c60f9..ce9ee18 100644 --- a/src/main/cpp/commands/intake/IntakeCommand.cpp +++ b/src/main/cpp/commands/intake/IntakeCommand.cpp @@ -13,20 +13,19 @@ IntakeCommand::IntakeCommand(std::shared_ptr 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() { diff --git a/src/main/cpp/commands/intake/IntakePositionCommand.cpp b/src/main/cpp/commands/intake/IntakePositionCommand.cpp new file mode 100644 index 0000000..7344e14 --- /dev/null +++ b/src/main/cpp/commands/intake/IntakePositionCommand.cpp @@ -0,0 +1,33 @@ +#include +#include + +#include "Utils.h" +#include "subsystems/Intake.h" + +#include "commands/intake/IntakePositionCommand.h" + +IntakePositionCommand::IntakePositionCommand(std::shared_ptr 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; +} \ No newline at end of file diff --git a/src/main/cpp/io/IntakeIO.cpp b/src/main/cpp/io/IntakeIO.cpp index 37875f2..5e65d4d 100644 --- a/src/main/cpp/io/IntakeIO.cpp +++ b/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); diff --git a/src/main/cpp/subsystems/Intake.cpp b/src/main/cpp/subsystems/Intake.cpp index 5221927..dd20217 100644 --- a/src/main/cpp/subsystems/Intake.cpp +++ b/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(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)); } diff --git a/src/main/cpp/ui/UserInterface.cpp b/src/main/cpp/ui/UserInterface.cpp index 6db5996..247be75 100644 --- a/src/main/cpp/ui/UserInterface.cpp +++ b/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(); } \ No newline at end of file diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index 6195aae..01da3b6 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -107,3 +107,6 @@ cone = 5 kP = 0.01 kI = 0 kD = 0 + +[intake] +tilt_offset = 0.0 \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 8a5b258..b9d588a 100644 --- a/src/main/include/RobotContainer.h +++ b/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 @@ -182,4 +184,7 @@ class RobotContainer { std::shared_ptr intake_command; std::shared_ptr other_intake_command; std::shared_ptr stop_intake_command; + + std::shared_ptr inc_wrist_tilt_command_; + std::shared_ptr dec_wrist_tilt_command_; }; diff --git a/src/main/include/commands/intake/IncrementIntakePositionCommand.h b/src/main/include/commands/intake/IncrementIntakePositionCommand.h new file mode 100644 index 0000000..da4681d --- /dev/null +++ b/src/main/include/commands/intake/IncrementIntakePositionCommand.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +#include "Utils.h" +#include "subsystems/Intake.h" + +/** + * + */ +class IncrementIntakePositionCommand + : public frc2::CommandHelper { +public: + /** + * Creates a new IncrementArmExtendCommand. + * + * @param arm The subsystem used by this command. + */ + explicit IncrementIntakePositionCommand(std::shared_ptr intake, double angle); + + void Execute() override; + void Initialize() override; + bool IsFinished() override; + +private: + std::shared_ptr intake_; + double angle_; +}; \ No newline at end of file diff --git a/src/main/include/commands/intake/IntakeCommand.h b/src/main/include/commands/intake/IntakeCommand.h index e4a71cf..a6e1759 100644 --- a/src/main/include/commands/intake/IntakeCommand.h +++ b/src/main/include/commands/intake/IntakeCommand.h @@ -20,6 +20,7 @@ class IntakeCommand explicit IntakeCommand(std::shared_ptr intake, double power); void Execute() override; + void Initialize() override; bool IsFinished() override; private: diff --git a/src/main/include/commands/intake/IntakePositionCommand.h b/src/main/include/commands/intake/IntakePositionCommand.h new file mode 100644 index 0000000..1d2ec17 --- /dev/null +++ b/src/main/include/commands/intake/IntakePositionCommand.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +#include "Utils.h" +#include "subsystems/Intake.h" + +/** + * + */ +class IntakePositionCommand + : public frc2::CommandHelper { +public: + /** + * Creates a new IncrementArmExtendCommand. + * + * @param arm The subsystem used by this command. + */ + explicit IntakePositionCommand(std::shared_ptr intake, double angle); + + void Execute() override; + void Initialize() override; + bool IsFinished() override; + +private: + std::shared_ptr intake_; + double angle_; +}; \ No newline at end of file diff --git a/src/main/include/io/IntakeIO.h b/src/main/include/io/IntakeIO.h index 576715c..d647b83 100644 --- a/src/main/include/io/IntakeIO.h +++ b/src/main/include/io/IntakeIO.h @@ -73,4 +73,5 @@ class IntakeIO : public frc2::SubsystemBase { IntakeSoftwareInterface *const sw_interface_; double max_output; + double offset_; }; \ No newline at end of file diff --git a/src/main/include/subsystems/Intake.h b/src/main/include/subsystems/Intake.h index fbc8858..bd7232c 100644 --- a/src/main/include/subsystems/Intake.h +++ b/src/main/include/subsystems/Intake.h @@ -8,6 +8,7 @@ #include "Logging.h" #include "wpi/DataLog.h" +#include "ui/UserInterface.h" class Intake : public frc2::SubsystemBase { public: @@ -18,6 +19,7 @@ class Intake : public frc2::SubsystemBase { bool SetIntakePower(double power); bool SetIntakeTilt(double tilt); + bool IncrementIntakeTilt(double inc); bool SetControlMode(const ControlMode &mode); bool ManualControl(); diff --git a/src/main/include/ui/UserInterface.h b/src/main/include/ui/UserInterface.h index e4f36f0..3b0b2cb 100644 --- a/src/main/include/ui/UserInterface.h +++ b/src/main/include/ui/UserInterface.h @@ -66,4 +66,10 @@ namespace ClawUI { extern frc::ShuffleboardTab &nt_tab; extern nt::GenericEntry *const nt_claw_encoder; +} + +namespace IntakeUI { + extern frc::ShuffleboardTab &nt_tab; + + extern nt::GenericEntry *const nt_tilt; } \ No newline at end of file From 298a0216ddb035beafca101eb1d6ed0e8e1c95a5 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 26 Mar 2023 10:10:34 -0500 Subject: [PATCH 05/19] fix intake wrist tilt encoder port --- src/main/include/hardware/Sensors.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/include/hardware/Sensors.h b/src/main/include/hardware/Sensors.h index 4b3ea3a..8a34a53 100644 --- a/src/main/include/hardware/Sensors.h +++ b/src/main/include/hardware/Sensors.h @@ -18,7 +18,7 @@ // DIO ports (on the RIO) #define EXTEND_LIMIT_SWITCH 9 #define ARM_ABS_ENCODER 1 -#define WRIST_ABS_ENCODER 4 +#define WRIST_ABS_ENCODER 3 typedef struct sensors_t { // navX IMU From 48aaeedbec54be1df676907d0955d94c9c7cdb48 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 26 Mar 2023 10:11:38 -0500 Subject: [PATCH 06/19] log the intake tilt output, setpoint, and sensor reading --- src/main/include/subsystems/Intake.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/include/subsystems/Intake.h b/src/main/include/subsystems/Intake.h index bd7232c..0ad59d4 100644 --- a/src/main/include/subsystems/Intake.h +++ b/src/main/include/subsystems/Intake.h @@ -9,6 +9,8 @@ #include "Logging.h" #include "wpi/DataLog.h" #include "ui/UserInterface.h" +#include "wpi/DataLog.h" + class Intake : public frc2::SubsystemBase { public: @@ -37,5 +39,9 @@ class Intake : public frc2::SubsystemBase { double intake_power_; - ControlMode mode_; + ControlMode mode_; + + wpi::log::DoubleLogEntry setpoint_log_; + wpi::log::DoubleLogEntry output_log_; + wpi::log::DoubleLogEntry encoder_log_; }; From b6f74a77cab48c73532845ac871ef616b58363c6 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 26 Mar 2023 10:12:27 -0500 Subject: [PATCH 07/19] field-oriented intake tilting commands --- src/main/cpp/RobotContainer.cpp | 28 +++++++++- .../intake/FieldOrientedIntakeCommand.cpp | 53 +++++++++++++++++++ src/main/include/RobotContainer.h | 9 +++- .../intake/FieldOrientedIntakeCommand.h | 32 +++++++++++ 4 files changed, 119 insertions(+), 3 deletions(-) create mode 100644 src/main/cpp/commands/intake/FieldOrientedIntakeCommand.cpp create mode 100644 src/main/include/commands/intake/FieldOrientedIntakeCommand.h diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 25e446d..f658d12 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -63,6 +63,9 @@ bool RobotContainer::ConfigureButtonBindings() { // driver_x_button_->WhileActiveContinous(*retractArmCommand); // driver_b_button_->WhileActiveContinous(*extendArmCommand); + + // manip_left_stick_button_->WhileHeld(*inc_wrist_tilt_command_); + // manip_right_stick_button_->WhileHeld(*dec_wrist_tilt_command_); // second driver controls manip_right_bumper_button_->WhenPressed(*arm_pickup_reverse_command_); @@ -72,8 +75,12 @@ 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_); + manip_right_bumper_button_->WhenPressed(*tilt_pickup_reverse_command_); + manip_left_bumper_button_->WhenPressed(*tilt_pickup_command_); + manip_x_button_->WhenPressed(*tilt_carry_command_); + + manip_b_button_->WhenPressed(*tilt_mid_command_); + manip_y_button_->WhenPressed(*tilt_high_command_); WPI_UNIGNORE_DEPRECATED @@ -247,6 +254,7 @@ bool RobotContainer::InitIntake() { intake_io_ = std::make_shared(intake_hw_.get(), intake_sw_.get()); OKC_CHECK(intake_io_ != nullptr); + OKC_CALL(intake_io_->Init()); intake_ = std::make_shared(intake_sw_.get()); @@ -315,6 +323,15 @@ bool RobotContainer::InitCommands() { double negative_score_high_rotation_ = RobotParams::GetParam("arm.negative_score_high.arm_setpoint", 0.0); double negative_score_high_extension_ = RobotParams::GetParam("arm.negative_score_high.extend_setpoint", 1.0); + double tilt_pickup_reverse_ = RobotParams::GetParam("arm.pickup.intake_setpoint", 0.0); + double tilt_pickup_ = RobotParams::GetParam("arm.negative_pickup.intake_setpoint", 0.0); + + double tilt_score_mid_ = RobotParams::GetParam("arm.score_medium.intake_setpoint", 0.0); + double tilt_score_mid_reverse_ = RobotParams::GetParam("arm.negative_score_medium.intake_setpoint", 0.0); + + double tilt_score_high_ = RobotParams::GetParam("arm.score_high.intake_setpoint", 0.0); + double tilt_score_high_reverse_ = RobotParams::GetParam("arm.negative_score_high.intake_setpoint", 0.0); + // autons score_preload_backup_auto_ = std::make_shared(swerve_drive_, arm_, intake_); @@ -349,6 +366,13 @@ bool RobotContainer::InitCommands() { inc_wrist_tilt_command_ = std::make_shared(intake_, 1); dec_wrist_tilt_command_ = std::make_shared(intake_, -1); + + tilt_pickup_reverse_command_ = std::make_shared(intake_, tilt_pickup_reverse_); + tilt_pickup_command_ = std::make_shared(intake_, tilt_pickup_); + tilt_carry_command_ = std::make_shared(intake_, 0.0); + + tilt_mid_command_ = std::make_shared(intake_, swerve_drive_, tilt_score_mid_, tilt_score_mid_reverse_); + tilt_high_command_ = std::make_shared(intake_, swerve_drive_, tilt_score_high_, tilt_score_high_reverse_); return true; } diff --git a/src/main/cpp/commands/intake/FieldOrientedIntakeCommand.cpp b/src/main/cpp/commands/intake/FieldOrientedIntakeCommand.cpp new file mode 100644 index 0000000..243c957 --- /dev/null +++ b/src/main/cpp/commands/intake/FieldOrientedIntakeCommand.cpp @@ -0,0 +1,53 @@ +#include +#include + +#include "Utils.h" +#include "subsystems/Intake.h" + +#include "commands/intake/FieldOrientedIntakeCommand.h" + +FieldOrientedIntakeCommand::FieldOrientedIntakeCommand(std::shared_ptr intake, std::shared_ptr swerve, double angle, double reverse_angle) { + // Set everything. + intake_ = intake; + angle_ = angle; + swerve_drive_ = swerve; + reverse_angle_ = reverse_angle; + + if (intake_ != nullptr) { + this->AddRequirements(intake_.get()); + } +} + +void FieldOrientedIntakeCommand::Initialize() { + VOKC_CHECK(intake_ != nullptr) + VOKC_CALL(intake_->SetControlMode(Auto)); +} + + +void FieldOrientedIntakeCommand::Execute() { + VOKC_CHECK(intake_ != nullptr); + + // get the current robot heading + double heading = 0.0; + VOKC_CALL(swerve_drive_->GetHeading(&heading)); + + // convert to [-180, 180] + TeamOKC::WrapAngle(&heading); + + // if the heading is pointing away from the scoring area + if (abs(heading) < 90.0) { + // score on the normal side of the robot + // set the desired state of the arm + VOKC_CALL(intake_->SetIntakeTilt(angle_)); + } else { + // otherwise, score on the other side + // set the desired state of the arm + VOKC_CALL(intake_->SetIntakeTilt(reverse_angle_)); + } + +} + +bool FieldOrientedIntakeCommand::IsFinished() { + // Always end this command. + return true; +} \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index b9d588a..a2a575e 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -45,11 +45,11 @@ #include "commands/arm/ArmFieldOrientedCommand.h" #include "commands/arm/ArmSetStateCommand.h" - //intake #include "commands/intake/IntakeCommand.h" #include "commands/intake/IntakePositionCommand.h" #include "commands/intake/IncrementIntakePositionCommand.h" +#include "commands/intake/FieldOrientedIntakeCommand.h" // misc #include @@ -187,4 +187,11 @@ class RobotContainer { std::shared_ptr inc_wrist_tilt_command_; std::shared_ptr dec_wrist_tilt_command_; + + std::shared_ptr tilt_pickup_reverse_command_; + std::shared_ptr tilt_pickup_command_; + std::shared_ptr tilt_carry_command_; + + std::shared_ptr tilt_mid_command_; + std::shared_ptr tilt_high_command_; }; diff --git a/src/main/include/commands/intake/FieldOrientedIntakeCommand.h b/src/main/include/commands/intake/FieldOrientedIntakeCommand.h new file mode 100644 index 0000000..bb8ecdf --- /dev/null +++ b/src/main/include/commands/intake/FieldOrientedIntakeCommand.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +#include "Utils.h" +#include "subsystems/Intake.h" +#include "subsystems/SwerveDrive.h" + +/** + * + */ +class FieldOrientedIntakeCommand + : public frc2::CommandHelper { +public: + /** + * Creates a new IncrementArmExtendCommand. + * + * @param arm The subsystem used by this command. + */ + explicit FieldOrientedIntakeCommand(std::shared_ptr intake, std::shared_ptr swerve, double angle, double reverse_angle); + + void Execute() override; + void Initialize() override; + bool IsFinished() override; + +private: + std::shared_ptr intake_; + std::shared_ptr swerve_drive_; + double angle_; + double reverse_angle_; +}; \ No newline at end of file From 0050fb7a9c9ebce2149af7c3122dbed314eab07a Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 26 Mar 2023 10:13:09 -0500 Subject: [PATCH 08/19] wrap the tilt encoder because the zero-point is in a really annoying spot and fix the setpoints --- src/main/cpp/io/IntakeIO.cpp | 3 ++- src/main/deploy/parameters.toml | 37 +++++++++++++++++++++------------ 2 files changed, 26 insertions(+), 14 deletions(-) diff --git a/src/main/cpp/io/IntakeIO.cpp b/src/main/cpp/io/IntakeIO.cpp index 5e65d4d..c11d8f3 100644 --- a/src/main/cpp/io/IntakeIO.cpp +++ b/src/main/cpp/io/IntakeIO.cpp @@ -45,7 +45,8 @@ bool IntakeIO::ProcessIO() { // wrist encoder OKC_CHECK(hw_interface_->wrist_encoder != nullptr); - sw_interface_->tilt_encoder = hw_interface_->wrist_encoder->GetAbsolutePosition() + offset_; + sw_interface_->tilt_encoder = hw_interface_->wrist_encoder->GetAbsolutePosition() * 360.0 + offset_; + TeamOKC::WrapAngle(&sw_interface_->tilt_encoder); // === outputs === hw_interface_->intake_motor->Set(sw_interface_->intake_power); diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index 01da3b6..5a82ee0 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -55,37 +55,43 @@ right_front_offset = 10.0 right_back_offset = 153.0 [arm] -offset = -390 +offset = -372 lift_limit = 110 extend_limit = 95 max_output = 1 arm_open_loop = 1 extend_open_loop = 0.5 -# right in front of the robot to pick stuff up +# right in front of the robot to pick stuff up0 [arm.pickup] -extend_setpoint = 79.5 -arm_setpoint = 31.0 +extend_setpoint = 20.0 +arm_setpoint = 33.0 +intake_setpoint = -70.0 [arm.score_high] -extend_setpoint = 90.5 -arm_setpoint = 94.0 +extend_setpoint = 50.0 +arm_setpoint = 103.0 +intake_setpoint = 88.0 [arm.score_medium] -extend_setpoint = 61.5 -arm_setpoint = 72.0 +extend_setpoint = 20.0 +arm_setpoint = 94.0 +intake_setpoint = 40.0 [arm.negative_pickup] -extend_setpoint = 88.5 -arm_setpoint = -45.0 +extend_setpoint = 20.0 +arm_setpoint = -25.0 +intake_setpoint = -40.0 [arm.negative_score_high] -extend_setpoint = 90.5 +extend_setpoint = 50.0 arm_setpoint = -103.0 +intake_setpoint = -68.0 [arm.negative_score_medium] -extend_setpoint = 61.5 +extend_setpoint = 20.0 arm_setpoint = -80.0 +intake_setpoint = 40.0 [arm.lift_pid] @@ -109,4 +115,9 @@ kI = 0 kD = 0 [intake] -tilt_offset = 0.0 \ No newline at end of file +tilt_offset = -55.0 + +[intake.wrist_pid] +kP = 0.02 +kI = 0.0 +kD = 0.0 \ No newline at end of file From c7245a30ed6b7c0cd7bf75808fe8271ff1c1ce89 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 26 Mar 2023 10:13:44 -0500 Subject: [PATCH 09/19] log the intake wrist values, and show them on shuffleboard, --- process_wpilib_logs.py | 22 ++++++++++++++++++++++ shuffleboard.json | 20 ++++++++++++++++++-- src/main/cpp/subsystems/Intake.cpp | 11 ++++++++++- src/main/cpp/ui/UserInterface.cpp | 2 +- 4 files changed, 51 insertions(+), 4 deletions(-) diff --git a/process_wpilib_logs.py b/process_wpilib_logs.py index 5d5703b..7523571 100644 --- a/process_wpilib_logs.py +++ b/process_wpilib_logs.py @@ -22,6 +22,10 @@ joystickStrafeLog = [[], []] joystickTurnLog = [[], []] +tiltEncLog = [[], []] +tiltOutputLog = [[], []] +tiltSetpointLog = [[], []] + root = tkinter.Tk() root.withdraw() @@ -83,6 +87,16 @@ joystickTurnLog[0].append(float(timestamp)) joystickTurnLog[1].append(float(data)) + if (id == '"/tilt/output"'): + tiltOutputLog[0].append(float(timestamp)) + tiltOutputLog[1].append(float(data)) # so we can actually see it on the graph + elif id == '"/tilt/encoder"': + tiltEncLog[0].append(float(timestamp)) + tiltEncLog[1].append(float(data)) + elif id == '"/tilt/setpoint"': + tiltSetpointLog[0].append(float(timestamp)) + tiltSetpointLog[1].append(float(data)) + plt.figure() plt.plot(liftEncLog[0], liftEncLog[1]) plt.plot(liftOutputLog[0], liftOutputLog[1]) @@ -99,6 +113,14 @@ plt.ylabel("data") plt.legend(("extend enc", "output", "setpoint")) +plt.figure() +plt.plot(tiltEncLog[0], tiltEncLog[1]) +plt.plot(tiltOutputLog[0], tiltOutputLog[1]) +plt.plot(tiltSetpointLog[0], tiltSetpointLog[1]) +plt.xlabel("Time (sec)") +plt.ylabel("data") +plt.legend(("tilt enc", "tilt output", "tilt setpoint")) + plt.figure() plt.plot(joystickDriveLog[0], joystickDriveLog[1]) plt.plot(joystickStrafeLog[0], joystickStrafeLog[1]) diff --git a/shuffleboard.json b/shuffleboard.json index 9c09519..a331ff0 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -494,6 +494,22 @@ "_glyph": 148, "_showGlyph": false } + }, + "0,4": { + "size": [ + 3, + 2 + ], + "content": { + "_type": "Simple Dial", + "_source0": "network_table:///Shuffleboard/intake/wrist encoder", + "_title": "Shuffleboard/intake/wrist encoder", + "_glyph": 148, + "_showGlyph": false, + "Range/Min": -360.0, + "Range/Max": 360.0, + "Visuals/Show value": true + } } } } @@ -547,8 +563,8 @@ "Controls/Rotation": "NONE", "compression": -1.0, "fps": -1, - "imageWidth": 240, - "imageHeight": 120 + "imageWidth": 0, + "imageHeight": 0 } } } diff --git a/src/main/cpp/subsystems/Intake.cpp b/src/main/cpp/subsystems/Intake.cpp index dd20217..1ef2c7d 100644 --- a/src/main/cpp/subsystems/Intake.cpp +++ b/src/main/cpp/subsystems/Intake.cpp @@ -14,6 +14,10 @@ bool Intake::Init() { // wrist_pid_->SetTolerance(1, 1); + setpoint_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/tilt/setpoint"); + output_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/tilt/output"); + encoder_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/tilt/encoder"); + //TODO verify that this is a good idea this->wrist_pid_->SetSetpoint(0.0); @@ -68,7 +72,8 @@ bool Intake::AutoControl() { OKC_CHECK(interface_ != nullptr); OKC_CHECK(this->wrist_pid_ != nullptr); - interface_->tilt_power = this->wrist_pid_->Calculate(interface_->tilt_encoder); + interface_->tilt_power = -this->wrist_pid_->Calculate(interface_->tilt_encoder); + // TeamOKC::Clamp(-0.5, 0.5, &interface_->tilt_power); interface_->intake_power = intake_power_; return true; @@ -91,6 +96,10 @@ void Intake::Periodic() { } VOKC_CALL(IntakeUI::nt_tilt->SetDouble(this->interface_->tilt_encoder)); + + setpoint_log_.Append(this->wrist_pid_->GetSetpoint()); + output_log_.Append(this->interface_->tilt_power); + encoder_log_.Append(this->interface_->tilt_encoder); } diff --git a/src/main/cpp/ui/UserInterface.cpp b/src/main/cpp/ui/UserInterface.cpp index 247be75..440d7d8 100644 --- a/src/main/cpp/ui/UserInterface.cpp +++ b/src/main/cpp/ui/UserInterface.cpp @@ -68,7 +68,7 @@ namespace ClawUI { } namespace IntakeUI { - frc::ShuffleboardTab &nt_tab = frc::Shuffleboard::GetTab("claw"); + frc::ShuffleboardTab &nt_tab = frc::Shuffleboard::GetTab("intake"); nt::GenericEntry *const nt_tilt = nt_tab.Add("wrist encoder", 0.0).GetEntry(); } \ No newline at end of file From 917d6748316e6651f2d4c7e7369317eb0a9dbfaf Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 26 Mar 2023 13:09:17 -0500 Subject: [PATCH 10/19] if arm is close to 0 and trying to go to 0 then prevent it from oscillating --- src/main/cpp/subsystems/Arm.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/Arm.cpp b/src/main/cpp/subsystems/Arm.cpp index 2e30d3a..3a53fe2 100644 --- a/src/main/cpp/subsystems/Arm.cpp +++ b/src/main/cpp/subsystems/Arm.cpp @@ -250,7 +250,15 @@ bool Arm::AutoControl() { this->interface_->arm_extend_power = this->inches_pid_->Calculate(this->interface_->arm_extend_encoder); // and keep rotation where it is - this->interface_->arm_lift_power = this->arm_pid_->Calculate(this->interface_->arm_duty_cycle_encoder); + + // if the arm is trying to go to 0, and we're close to 0 + if (abs(this->desired_state_.rotation) < 2.0 && abs(this->state_.rotation) < 10.0) { + // to prevent it from oscillating just set it to 0 + this->interface_->arm_lift_power = 0.0; + } else { + // otherwise + this->interface_->arm_lift_power = this->arm_pid_->Calculate(this->interface_->arm_duty_cycle_encoder); + } } else { OKC_CHECK_MSG(false, "arm state machine unknown state"); } From 04c215f29a467a27fddcbbf085f10bf2e77d0f64 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 26 Mar 2023 13:20:12 -0500 Subject: [PATCH 11/19] added wrist setpoint to shuffleboard --- src/main/cpp/subsystems/Intake.cpp | 1 + src/main/cpp/ui/UserInterface.cpp | 1 + src/main/include/ui/UserInterface.h | 1 + 3 files changed, 3 insertions(+) diff --git a/src/main/cpp/subsystems/Intake.cpp b/src/main/cpp/subsystems/Intake.cpp index 1ef2c7d..067591c 100644 --- a/src/main/cpp/subsystems/Intake.cpp +++ b/src/main/cpp/subsystems/Intake.cpp @@ -96,6 +96,7 @@ void Intake::Periodic() { } VOKC_CALL(IntakeUI::nt_tilt->SetDouble(this->interface_->tilt_encoder)); + VOKC_CALL(IntakeUI::nt_tilt_setpoint->SetDouble(this->wrist_pid_->GetSetpoint())); setpoint_log_.Append(this->wrist_pid_->GetSetpoint()); output_log_.Append(this->interface_->tilt_power); diff --git a/src/main/cpp/ui/UserInterface.cpp b/src/main/cpp/ui/UserInterface.cpp index 440d7d8..86d3b2b 100644 --- a/src/main/cpp/ui/UserInterface.cpp +++ b/src/main/cpp/ui/UserInterface.cpp @@ -71,4 +71,5 @@ namespace IntakeUI { frc::ShuffleboardTab &nt_tab = frc::Shuffleboard::GetTab("intake"); nt::GenericEntry *const nt_tilt = nt_tab.Add("wrist encoder", 0.0).GetEntry(); + nt::GenericEntry *const nt_tilt_setpoint = nt_tab.Add("wrist setpoint", 0.0).GetEntry(); } \ No newline at end of file diff --git a/src/main/include/ui/UserInterface.h b/src/main/include/ui/UserInterface.h index 3b0b2cb..f6c676b 100644 --- a/src/main/include/ui/UserInterface.h +++ b/src/main/include/ui/UserInterface.h @@ -72,4 +72,5 @@ namespace IntakeUI { extern frc::ShuffleboardTab &nt_tab; extern nt::GenericEntry *const nt_tilt; + extern nt::GenericEntry *const nt_tilt_setpoint; } \ No newline at end of file From e8aad744806644d8642a9f14f39ba989ff5aab92 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 26 Mar 2023 13:21:02 -0500 Subject: [PATCH 12/19] made intake faster --- src/main/cpp/RobotContainer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index f658d12..d9498ab 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -360,8 +360,8 @@ bool RobotContainer::InitCommands() { arm_score_high_command_ = std::make_shared(arm_, swerve_drive_, TeamOKC::ArmState(score_high_extension_, score_high_rotation_), TeamOKC::ArmState(negative_score_high_extension_, negative_score_high_rotation_)); // intake commands - intake_command = std::make_shared(intake_, 0.3); - other_intake_command = std::make_shared(intake_, -0.3); + intake_command = std::make_shared(intake_, 0.5); + other_intake_command = std::make_shared(intake_, -0.5); stop_intake_command = std::make_shared(intake_, -0.01); inc_wrist_tilt_command_ = std::make_shared(intake_, 1); From 895e017dc6f533aae4baf6a055b219deb909c4c3 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Sun, 26 Mar 2023 13:44:26 -0500 Subject: [PATCH 13/19] started work on a command to pause auton execution for the arm --- .../cpp/commands/arm/WaitForArmCommand.cpp | 27 +++++++++++++++++++ .../include/commands/arm/WaitForArmCommand.h | 25 +++++++++++++++++ 2 files changed, 52 insertions(+) create mode 100644 src/main/cpp/commands/arm/WaitForArmCommand.cpp create mode 100644 src/main/include/commands/arm/WaitForArmCommand.h diff --git a/src/main/cpp/commands/arm/WaitForArmCommand.cpp b/src/main/cpp/commands/arm/WaitForArmCommand.cpp new file mode 100644 index 0000000..a8a7332 --- /dev/null +++ b/src/main/cpp/commands/arm/WaitForArmCommand.cpp @@ -0,0 +1,27 @@ +#include +#include + +#include "Utils.h" +#include "subsystems/Arm.h" + +#include "commands/arm/WaitForArmCommand.h" + + + +WaitForArmCommand::WaitForArmCommand(std::shared_ptr arm) { + // Set everything. + arm_ = arm; + + if (arm_ != nullptr) { + this->AddRequirements(arm_.get()); + } +} + +void WaitForArmCommand::Execute() { + VOKC_CHECK(arm_ != nullptr); + +} + +bool WaitForArmCommand::IsFinished() { + return false; +} \ No newline at end of file diff --git a/src/main/include/commands/arm/WaitForArmCommand.h b/src/main/include/commands/arm/WaitForArmCommand.h new file mode 100644 index 0000000..8d43d03 --- /dev/null +++ b/src/main/include/commands/arm/WaitForArmCommand.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +#include "Utils.h" +#include "subsystems/Arm.h" + +/** + * Move arm to a preset position + */ +class WaitForArmCommand + : public frc2::CommandHelper { +public: + /** + * @param subsystem The subsystem used by this command. + */ + explicit WaitForArmCommand(std::shared_ptr arm); + + void Execute() override; + bool IsFinished() override; + +private: + std::shared_ptr arm_; +}; \ No newline at end of file From 0408639fc5d14014d1e78adca79225ade00ee7f4 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 27 Mar 2023 16:47:03 -0500 Subject: [PATCH 14/19] wrist setpoint on shuffleboard --- shuffleboard.json | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/shuffleboard.json b/shuffleboard.json index a331ff0..3cfded9 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -482,7 +482,7 @@ "_showGlyph": false } }, - "3,4": { + "4,5": { "size": [ 1, 1 @@ -510,6 +510,25 @@ "Range/Max": 360.0, "Visuals/Show value": true } + }, + "3,4": { + "size": [ + 1, + 2 + ], + "content": { + "_type": "Number Bar", + "_source0": "network_table:///Shuffleboard/intake/wrist setpoint", + "_title": "Shuffleboard/intake/wrist setpoint", + "_glyph": 148, + "_showGlyph": false, + "Range/Min": -100.0, + "Range/Max": 100.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true, + "Visuals/Orientation": "VERTICAL" + } } } } From fb8667cf7feee9188d32e59d373539c18bd39a51 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 27 Mar 2023 16:47:43 -0500 Subject: [PATCH 15/19] fix extend thresholds/setpoints because we replaced the gearboxes --- src/main/cpp/subsystems/Arm.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/cpp/subsystems/Arm.cpp b/src/main/cpp/subsystems/Arm.cpp index 3a53fe2..315602d 100644 --- a/src/main/cpp/subsystems/Arm.cpp +++ b/src/main/cpp/subsystems/Arm.cpp @@ -214,10 +214,10 @@ bool Arm::AutoControl() { // rotation takes priority, and if necessary the arm will be extended/retracted to reach a certain angle } else if (control_state_ == ROTATING) { // bring the extension in whenever we rotate the arm, to reduce bounce - this->inches_pid_->SetSetpoint(1); + this->inches_pid_->SetSetpoint(0.5); // if we have brought the extension in - if (abs(1.0 - state_.extension) < 2.0) { + if (abs(1.0 - state_.extension) < 1.0) { // then move the arm this->arm_pid_->SetSetpoint(this->desired_state_.rotation); this->interface_->arm_lift_power = this->arm_pid_->Calculate(this->interface_->arm_duty_cycle_encoder); @@ -252,7 +252,7 @@ bool Arm::AutoControl() { // and keep rotation where it is // if the arm is trying to go to 0, and we're close to 0 - if (abs(this->desired_state_.rotation) < 2.0 && abs(this->state_.rotation) < 10.0) { + if (abs(this->desired_state_.rotation) < 2.0 && abs(this->state_.rotation) < 7.0) { // to prevent it from oscillating just set it to 0 this->interface_->arm_lift_power = 0.0; } else { From 7b05742aed775e2e51595faee5c7f4e4df5eebef Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 27 Mar 2023 16:47:53 -0500 Subject: [PATCH 16/19] intake wrist tilt tuning and fixing --- src/main/cpp/RobotContainer.cpp | 8 +++---- .../cpp/autos/ScorePreloadedBalanceAuto.cpp | 18 +++++++++------ .../cpp/commands/intake/IntakeCommand.cpp | 2 +- src/main/deploy/parameters.toml | 22 +++++++++---------- 4 files changed, 27 insertions(+), 23 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index d9498ab..ac46802 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -323,8 +323,8 @@ bool RobotContainer::InitCommands() { double negative_score_high_rotation_ = RobotParams::GetParam("arm.negative_score_high.arm_setpoint", 0.0); double negative_score_high_extension_ = RobotParams::GetParam("arm.negative_score_high.extend_setpoint", 1.0); - double tilt_pickup_reverse_ = RobotParams::GetParam("arm.pickup.intake_setpoint", 0.0); - double tilt_pickup_ = RobotParams::GetParam("arm.negative_pickup.intake_setpoint", 0.0); + double tilt_pickup_reverse_ = RobotParams::GetParam("arm.negative_pickup.intake_setpoint", 0.0); + double tilt_pickup_ = RobotParams::GetParam("arm.pickup.intake_setpoint", 0.0); double tilt_score_mid_ = RobotParams::GetParam("arm.score_medium.intake_setpoint", 0.0); double tilt_score_mid_reverse_ = RobotParams::GetParam("arm.negative_score_medium.intake_setpoint", 0.0); @@ -360,8 +360,8 @@ bool RobotContainer::InitCommands() { arm_score_high_command_ = std::make_shared(arm_, swerve_drive_, TeamOKC::ArmState(score_high_extension_, score_high_rotation_), TeamOKC::ArmState(negative_score_high_extension_, negative_score_high_rotation_)); // intake commands - intake_command = std::make_shared(intake_, 0.5); - other_intake_command = std::make_shared(intake_, -0.5); + intake_command = std::make_shared(intake_, 1.0); + other_intake_command = std::make_shared(intake_, -1.0); stop_intake_command = std::make_shared(intake_, -0.01); inc_wrist_tilt_command_ = std::make_shared(intake_, 1); diff --git a/src/main/cpp/autos/ScorePreloadedBalanceAuto.cpp b/src/main/cpp/autos/ScorePreloadedBalanceAuto.cpp index d35d10d..374be33 100644 --- a/src/main/cpp/autos/ScorePreloadedBalanceAuto.cpp +++ b/src/main/cpp/autos/ScorePreloadedBalanceAuto.cpp @@ -4,6 +4,7 @@ #include "commands/intake/IntakeCommand.h" #include "commands/swerve/AutoBalanceCommand.h" #include "commands/arm/ArmSetStateCommand.h" +#include "commands/intake/IntakePositionCommand.h" #include "Parameters.h" ScorePreloadedBalanceAuto::ScorePreloadedBalanceAuto(std::shared_ptr swerve, std::shared_ptr arm, std::shared_ptr intake) { @@ -15,14 +16,17 @@ ScorePreloadedBalanceAuto::ScorePreloadedBalanceAuto(std::shared_ptr intake, void IntakeCommand::Initialize() { VOKC_CALL(intake_ != nullptr); - VOKC_CALL(intake_->SetControlMode(Manual)); + VOKC_CALL(intake_->SetControlMode(Auto)); } diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index 825049b..c7b2055 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -60,7 +60,7 @@ tilted_speed = 0.2 tilted_threshold = 15.0 reverse_threshold = 9.0 pitch_threshold = 1.0 -drive_backward_speed = -0.15 +drive_backward_speed = -0.2 timeout = 1.3 [arm] @@ -75,32 +75,32 @@ extend_open_loop = 0.5 [arm.pickup] extend_setpoint = 20.0 arm_setpoint = 33.0 -intake_setpoint = -70.0 +intake_setpoint = 40.0 [arm.score_high] -extend_setpoint = 50.0 -arm_setpoint = 103.0 +extend_setpoint = 60.0 +arm_setpoint = 110.0 intake_setpoint = 88.0 [arm.score_medium] extend_setpoint = 20.0 arm_setpoint = 94.0 -intake_setpoint = 40.0 +intake_setpoint = -40.0 [arm.negative_pickup] extend_setpoint = 20.0 arm_setpoint = -25.0 -intake_setpoint = -40.0 +intake_setpoint = -70.0 [arm.negative_score_high] extend_setpoint = 50.0 -arm_setpoint = -103.0 +arm_setpoint = -110.0 intake_setpoint = -68.0 [arm.negative_score_medium] -extend_setpoint = 20.0 -arm_setpoint = -80.0 -intake_setpoint = 40.0 +extend_setpoint = 15.0 +arm_setpoint = -100.0 +intake_setpoint = 52.0 [arm.lift_pid] @@ -129,4 +129,4 @@ tilt_offset = -55.0 [intake.wrist_pid] kP = 0.02 kI = 0.0 -kD = 0.0 \ No newline at end of file +kD = 0.0003 \ No newline at end of file From 3e82c1bdc029e0f869f9141ce09a13e652f51932 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Mon, 27 Mar 2023 18:16:11 -0500 Subject: [PATCH 17/19] code no workey tilt then move claw arm thingy --- build.gradle | 2 +- src/main/cpp/RobotContainer.cpp | 16 +++++---- .../commands/arm/TiltThenMoveArmCommand.cpp | 11 ++++++ .../intake/IntakeBlockingPositionCommand.cpp | 36 +++++++++++++++++++ src/main/cpp/subsystems/Intake.cpp | 14 +++++--- src/main/include/RobotContainer.h | 11 ++++-- .../commands/arm/TiltThenMoveArmCommand.h | 18 ++++++++++ .../intake/IntakeBlockingPositionCommand.h | 29 +++++++++++++++ src/main/include/subsystems/Intake.h | 1 + 9 files changed, 124 insertions(+), 14 deletions(-) create mode 100644 src/main/cpp/commands/arm/TiltThenMoveArmCommand.cpp create mode 100644 src/main/cpp/commands/intake/IntakeBlockingPositionCommand.cpp create mode 100644 src/main/include/commands/arm/TiltThenMoveArmCommand.h create mode 100644 src/main/include/commands/intake/IntakeBlockingPositionCommand.h diff --git a/build.gradle b/build.gradle index a55a04a..6edd6c5 100644 --- a/build.gradle +++ b/build.gradle @@ -36,7 +36,7 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcCpp // Set this to true to enable desktop support. -def includeDesktopSupport = true +def includeDesktopSupport = false // Set to true to run simulation in debug mode wpi.cpp.debugSimulation = false diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index ac46802..8374514 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -68,15 +68,13 @@ bool RobotContainer::ConfigureButtonBindings() { // manip_right_stick_button_->WhileHeld(*dec_wrist_tilt_command_); // second driver controls - manip_right_bumper_button_->WhenPressed(*arm_pickup_reverse_command_); - manip_left_bumper_button_->WhenPressed(*arm_pickup_command_); + manip_right_bumper_button_->WhenPressed(*pickup_command_); + manip_left_bumper_button_->WhenPressed(*pickup_reverse_command_); manip_x_button_->WhenPressed(*arm_carry_command_); manip_b_button_->WhenPressed(*arm_score_mid_command_); manip_y_button_->WhenPressed(*arm_score_high_command_); - manip_right_bumper_button_->WhenPressed(*tilt_pickup_reverse_command_); - manip_left_bumper_button_->WhenPressed(*tilt_pickup_command_); manip_x_button_->WhenPressed(*tilt_carry_command_); manip_b_button_->WhenPressed(*tilt_mid_command_); @@ -367,13 +365,17 @@ bool RobotContainer::InitCommands() { inc_wrist_tilt_command_ = std::make_shared(intake_, 1); dec_wrist_tilt_command_ = std::make_shared(intake_, -1); - tilt_pickup_reverse_command_ = std::make_shared(intake_, tilt_pickup_reverse_); - tilt_pickup_command_ = std::make_shared(intake_, tilt_pickup_); + tilt_pickup_reverse_command_ = std::make_shared(intake_, tilt_pickup_reverse_); + tilt_pickup_command_ = std::make_shared(intake_, tilt_pickup_); tilt_carry_command_ = std::make_shared(intake_, 0.0); tilt_mid_command_ = std::make_shared(intake_, swerve_drive_, tilt_score_mid_, tilt_score_mid_reverse_); tilt_high_command_ = std::make_shared(intake_, swerve_drive_, tilt_score_high_, tilt_score_high_reverse_); - + + // pickup commands + pickup_command_ = std::make_shared(tilt_pickup_command_, arm_pickup_command_); + pickup_reverse_command_ = std::make_shared(tilt_pickup_reverse_command_, arm_pickup_reverse_command_); + return true; } diff --git a/src/main/cpp/commands/arm/TiltThenMoveArmCommand.cpp b/src/main/cpp/commands/arm/TiltThenMoveArmCommand.cpp new file mode 100644 index 0000000..3b8354e --- /dev/null +++ b/src/main/cpp/commands/arm/TiltThenMoveArmCommand.cpp @@ -0,0 +1,11 @@ +#include "commands/arm/TiltThenMoveArmCommand.h" +#include "commands/intake/IntakePositionCommand.h" +#include "commands/arm/ArmSetStateCommand.h" + +TiltThenMoveArmCommand::TiltThenMoveArmCommand(std::shared_ptr move_intake, std::shared_ptr move_arm) { + + AddCommands( + // *move_intake, + // *move_arm + ); +} \ No newline at end of file diff --git a/src/main/cpp/commands/intake/IntakeBlockingPositionCommand.cpp b/src/main/cpp/commands/intake/IntakeBlockingPositionCommand.cpp new file mode 100644 index 0000000..fdccaec --- /dev/null +++ b/src/main/cpp/commands/intake/IntakeBlockingPositionCommand.cpp @@ -0,0 +1,36 @@ +#include +#include + +#include "Utils.h" +#include "subsystems/Intake.h" + +#include "commands/intake/IntakeBlockingPositionCommand.h" + +IntakeBlockingPositionCommand::IntakeBlockingPositionCommand(std::shared_ptr intake, double angle) { + // Set everything. + intake_ = intake; + angle_ = angle; + + if (intake_ != nullptr) { + this->AddRequirements(intake_.get()); + } +} + +void IntakeBlockingPositionCommand::Initialize() { + VOKC_CHECK(intake_ != nullptr) + VOKC_CALL(intake_->SetControlMode(Auto)); +} + + +void IntakeBlockingPositionCommand::Execute() { + VOKC_CHECK(intake_ != nullptr); + VOKC_CALL(intake_->SetIntakeTilt(angle_)); +} + +bool IntakeBlockingPositionCommand::IsFinished() { + bool at = false; + + OKC_CALL(intake_->AtSetpoint(&at)); + + return at; +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/Intake.cpp b/src/main/cpp/subsystems/Intake.cpp index 067591c..22612d9 100644 --- a/src/main/cpp/subsystems/Intake.cpp +++ b/src/main/cpp/subsystems/Intake.cpp @@ -11,15 +11,13 @@ bool Intake::Init() { 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(wrist_kP, wrist_kI, wrist_kD); + wrist_pid_->SetTolerance(2, 2); - // wrist_pid_->SetTolerance(1, 1); - setpoint_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/tilt/setpoint"); output_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/tilt/output"); encoder_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/tilt/encoder"); - //TODO verify that this is a good idea - this->wrist_pid_->SetSetpoint(0.0); + wrist_pid_->SetSetpoint(0.0); return true; } @@ -79,6 +77,14 @@ bool Intake::AutoControl() { return true; } +bool Intake::AtSetpoint(bool *at) { + OKC_CHECK(interface_ != nullptr); + + *at = wrist_pid_->AtSetpoint(); + + return true; +} + void Intake::SimulationPeriodic() { } diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index a2a575e..d9afcf3 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -44,12 +44,14 @@ #include "commands/arm/IncrementArmExtendCommand.h" #include "commands/arm/ArmFieldOrientedCommand.h" #include "commands/arm/ArmSetStateCommand.h" +#include "commands/arm/TiltThenMoveArmCommand.h" //intake #include "commands/intake/IntakeCommand.h" #include "commands/intake/IntakePositionCommand.h" #include "commands/intake/IncrementIntakePositionCommand.h" #include "commands/intake/FieldOrientedIntakeCommand.h" +#include "commands/intake/IntakeBlockingPositionCommand.h" // misc #include @@ -176,6 +178,7 @@ class RobotContainer { std::shared_ptr arm_carry_command_; std::shared_ptr arm_pickup_command_; + std::shared_ptr arm_pickup_reverse_command_; std::shared_ptr arm_score_mid_command_; std::shared_ptr arm_score_high_command_; @@ -188,10 +191,14 @@ class RobotContainer { std::shared_ptr inc_wrist_tilt_command_; std::shared_ptr dec_wrist_tilt_command_; - std::shared_ptr tilt_pickup_reverse_command_; - std::shared_ptr tilt_pickup_command_; + std::shared_ptr tilt_pickup_reverse_command_; + std::shared_ptr tilt_pickup_command_; std::shared_ptr tilt_carry_command_; std::shared_ptr tilt_mid_command_; std::shared_ptr tilt_high_command_; + + // pickup commands + std::shared_ptr pickup_command_; + std::shared_ptr pickup_reverse_command_; }; diff --git a/src/main/include/commands/arm/TiltThenMoveArmCommand.h b/src/main/include/commands/arm/TiltThenMoveArmCommand.h new file mode 100644 index 0000000..7304369 --- /dev/null +++ b/src/main/include/commands/arm/TiltThenMoveArmCommand.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +#include "commands/intake/IntakeBlockingPositionCommand.h" +#include "commands/arm/ArmSetStateCommand.h" + +/** + * Move arm to a preset position + */ +class TiltThenMoveArmCommand + : public frc2::CommandHelper { +public: + explicit TiltThenMoveArmCommand(std::shared_ptr move_intake, std::shared_ptr move_arm); + +private: +}; \ No newline at end of file diff --git a/src/main/include/commands/intake/IntakeBlockingPositionCommand.h b/src/main/include/commands/intake/IntakeBlockingPositionCommand.h new file mode 100644 index 0000000..5ef2288 --- /dev/null +++ b/src/main/include/commands/intake/IntakeBlockingPositionCommand.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +#include "Utils.h" +#include "subsystems/Intake.h" + +/** + * + */ +class IntakeBlockingPositionCommand + : public frc2::CommandHelper { +public: + /** + * Creates a new IncrementArmExtendCommand. + * + * @param arm The subsystem used by this command. + */ + explicit IntakeBlockingPositionCommand(std::shared_ptr intake, double angle); + + void Execute() override; + void Initialize() override; + bool IsFinished() override; + +private: + std::shared_ptr intake_; + double angle_; +}; \ No newline at end of file diff --git a/src/main/include/subsystems/Intake.h b/src/main/include/subsystems/Intake.h index 0ad59d4..5be2bda 100644 --- a/src/main/include/subsystems/Intake.h +++ b/src/main/include/subsystems/Intake.h @@ -26,6 +26,7 @@ class Intake : public frc2::SubsystemBase { bool SetControlMode(const ControlMode &mode); bool ManualControl(); bool AutoControl(); + bool AtSetpoint(bool *at); bool Reset(); From 563bdf344f2843d2c21b7965700fbb51d6f1330e Mon Sep 17 00:00:00 2001 From: deadlyvortex Date: Mon, 27 Mar 2023 19:11:26 -0500 Subject: [PATCH 18/19] Improved and Tuned Arm and Added ff to arm control --- src/main/cpp/Utils.cpp | 11 +++++++++++ src/main/cpp/subsystems/Arm.cpp | 8 +++++++- src/main/include/Utils.h | 1 + src/main/include/subsystems/Arm.h | 2 ++ 4 files changed, 21 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/Utils.cpp b/src/main/cpp/Utils.cpp index 89ac0bd..c01593b 100644 --- a/src/main/cpp/Utils.cpp +++ b/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 diff --git a/src/main/cpp/subsystems/Arm.cpp b/src/main/cpp/subsystems/Arm.cpp index 315602d..77b4da7 100644 --- a/src/main/cpp/subsystems/Arm.cpp +++ b/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(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; @@ -147,7 +152,8 @@ bool Arm::TestControl() { this->inches_pid_->SetSetpoint(this->desired_state_.extension); // set output - this->interface_->arm_lift_power = this->arm_pid_->Calculate(this->state_.rotation); + double ff = TeamOKC::sign(this->desired_state_.rotation) * arm_kF_ * this->desired_state_.rotation * this->desired_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; diff --git a/src/main/include/Utils.h b/src/main/include/Utils.h index 58f51fa..3925075 100644 --- a/src/main/include/Utils.h +++ b/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; diff --git a/src/main/include/subsystems/Arm.h b/src/main/include/subsystems/Arm.h index 26feb99..ef670ff 100644 --- a/src/main/include/subsystems/Arm.h +++ b/src/main/include/subsystems/Arm.h @@ -4,6 +4,7 @@ #include #include #include +#include "Utils.h" #include "Logging.h" #include "wpi/DataLog.h" @@ -55,6 +56,7 @@ class Arm : public frc2::SubsystemBase { // PID controllers std::shared_ptr arm_pid_; std::shared_ptr inches_pid_; + double arm_kF_; // controls stuff (I'd call it a state machine but it isn't really a state machine) ControlMode mode_; From 8c1d3f630d68950adf6ff602f98f231c05441151 Mon Sep 17 00:00:00 2001 From: danielbrownmsm Date: Tue, 4 Apr 2023 16:09:13 -0500 Subject: [PATCH 19/19] fix PR comments --- build.gradle | 2 +- src/main/cpp/subsystems/Arm.cpp | 5 ++++- src/main/deploy/parameters.toml | 1 - 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/build.gradle b/build.gradle index 6edd6c5..a55a04a 100644 --- a/build.gradle +++ b/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 diff --git a/src/main/cpp/subsystems/Arm.cpp b/src/main/cpp/subsystems/Arm.cpp index fd254f0..d9b485d 100644 --- a/src/main/cpp/subsystems/Arm.cpp +++ b/src/main/cpp/subsystems/Arm.cpp @@ -151,8 +151,11 @@ bool Arm::TestControl() { this->arm_pid_->SetSetpoint(this->desired_state_.rotation); this->inches_pid_->SetSetpoint(this->desired_state_.extension); - // set output + // 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) + ff; this->interface_->arm_extend_power = this->inches_pid_->Calculate(this->state_.extension); diff --git a/src/main/deploy/parameters.toml b/src/main/deploy/parameters.toml index 3ab757f..decce57 100644 --- a/src/main/deploy/parameters.toml +++ b/src/main/deploy/parameters.toml @@ -89,7 +89,6 @@ intake_setpoint = -40.0 [arm.negative_pickup] extend_setpoint = 20.0 -extend_setpoint = 50.0 arm_setpoint = -25.0 intake_setpoint = -70.0