From 8a21dd4d4700d3c50a9c742509ce799901855b0d Mon Sep 17 00:00:00 2001 From: okcroboticsteam Date: Sat, 8 Apr 2023 13:59:34 -0500 Subject: [PATCH 1/2] fixed test mode --- src/main/cpp/subsystems/Arm.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/subsystems/Arm.cpp b/src/main/cpp/subsystems/Arm.cpp index cbd50f6..d97eddd 100644 --- a/src/main/cpp/subsystems/Arm.cpp +++ b/src/main/cpp/subsystems/Arm.cpp @@ -276,8 +276,7 @@ bool Arm::AutoControl() { } void Arm::Periodic() { - - if (ArmUI::nt_test_mode->GetBoolean(false)) { + if (ArmUI::nt_test_mode->GetBoolean(false)) { mode_ = Test; } else if (ArmUI::nt_manual_arm_mode->GetBoolean(false)) { mode_ = Manual; @@ -293,6 +292,9 @@ void Arm::Periodic() { case Manual: VOKC_CALL(this->ManualControl()); break; + case Test: + VOKC_CALL(this->TestControl()); + break; default: VOKC_CHECK_MSG(false, "Unhandled enum"); } From b11dc9459662210a9a27bcc09243efcdbb3aa5f2 Mon Sep 17 00:00:00 2001 From: okcroboticsteam Date: Sat, 8 Apr 2023 15:31:22 -0500 Subject: [PATCH 2/2] tried to stop oscillation --- src/main/cpp/Robot.cpp | 6 ---- src/main/cpp/RobotContainer.cpp | 8 ++--- .../commands/arm/ArmFieldOrientedCommand.cpp | 2 +- .../cpp/commands/arm/ArmSetStateCommand.cpp | 2 +- .../commands/arm/ArmSetStateDpadCommand.cpp | 2 +- .../arm/IncrementArmExtendCommand.cpp | 2 +- .../arm/IncrementArmPresetPositionCommand.cpp | 6 ++-- src/main/cpp/subsystems/Arm.cpp | 32 ++++++++++--------- src/main/include/subsystems/Arm.h | 1 + 9 files changed, 28 insertions(+), 33 deletions(-) diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 6236ae8..b85f1b7 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -87,12 +87,6 @@ void Robot::TestPeriodic() { } void Robot::TestInit() { - m_container.GetArm()->SetControlMode(Test); - - teleop_command_ = m_container.GetDriveCommand(); - VOKC_CALL(teleop_command_ != nullptr); - teleop_command_->Schedule(); - VOKC_CHECK_MSG(false, "we are in fact reaching test init"); } /** diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 545433b..ca5ccd5 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -58,11 +58,11 @@ bool RobotContainer::ConfigureButtonBindings() { driver_right_bumper_->WhenPressed(*other_intake_command).WhenReleased(*stop_intake_command); // HACK XXX BUG TODO temporary first driver controls arm stuff for testing so only one person is needed to test the robot - // driver_a_button_->WhileActiveContinous(*lowerArmCommand); - // driver_y_button_->WhileActiveContinous(*raiseArmCommand); + manip_a_button_->WhileActiveContinous(*lowerArmCommand); + manip_y_button_->WhileActiveContinous(*raiseArmCommand); - // driver_x_button_->WhileActiveContinous(*retractArmCommand); - // driver_b_button_->WhileActiveContinous(*extendArmCommand); + manip_x_button_->WhileActiveContinous(*retractArmCommand); + manip_b_button_->WhileActiveContinous(*extendArmCommand); // second driver controls diff --git a/src/main/cpp/commands/arm/ArmFieldOrientedCommand.cpp b/src/main/cpp/commands/arm/ArmFieldOrientedCommand.cpp index 65c5c6a..bcbe6ec 100644 --- a/src/main/cpp/commands/arm/ArmFieldOrientedCommand.cpp +++ b/src/main/cpp/commands/arm/ArmFieldOrientedCommand.cpp @@ -23,7 +23,7 @@ ArmFieldOrientedCommand::ArmFieldOrientedCommand(std::shared_ptr arm, std:: void ArmFieldOrientedCommand::Initialize() { VOKC_CHECK(arm_ != nullptr); - VOKC_CALL(arm_->SetControlMode(Auto)); + } void ArmFieldOrientedCommand::Execute() { diff --git a/src/main/cpp/commands/arm/ArmSetStateCommand.cpp b/src/main/cpp/commands/arm/ArmSetStateCommand.cpp index 3d4f276..11efd42 100644 --- a/src/main/cpp/commands/arm/ArmSetStateCommand.cpp +++ b/src/main/cpp/commands/arm/ArmSetStateCommand.cpp @@ -21,7 +21,7 @@ ArmSetStateCommand::ArmSetStateCommand(std::shared_ptr arm, TeamOKC::ArmSta void ArmSetStateCommand::Initialize() { VOKC_CHECK(arm_ != nullptr); - VOKC_CALL(arm_->SetControlMode(Auto)); + } void ArmSetStateCommand::Execute() { diff --git a/src/main/cpp/commands/arm/ArmSetStateDpadCommand.cpp b/src/main/cpp/commands/arm/ArmSetStateDpadCommand.cpp index 7ad0185..7d57af4 100644 --- a/src/main/cpp/commands/arm/ArmSetStateDpadCommand.cpp +++ b/src/main/cpp/commands/arm/ArmSetStateDpadCommand.cpp @@ -31,7 +31,7 @@ ArmSetStateDpadCommand::ArmSetStateDpadCommand(std::shared_ptr arm, std::sh void ArmSetStateDpadCommand::Initialize() { VOKC_CHECK(arm_ != nullptr); - VOKC_CALL(arm_->SetControlMode(Auto)); + } void ArmSetStateDpadCommand::Execute() { diff --git a/src/main/cpp/commands/arm/IncrementArmExtendCommand.cpp b/src/main/cpp/commands/arm/IncrementArmExtendCommand.cpp index cd91d9d..5a5d235 100644 --- a/src/main/cpp/commands/arm/IncrementArmExtendCommand.cpp +++ b/src/main/cpp/commands/arm/IncrementArmExtendCommand.cpp @@ -18,7 +18,7 @@ IncrementArmExtendCommand::IncrementArmExtendCommand(std::shared_ptr arm, } void IncrementArmExtendCommand::Initialize() { - arm_->SetControlMode(Test); + } diff --git a/src/main/cpp/commands/arm/IncrementArmPresetPositionCommand.cpp b/src/main/cpp/commands/arm/IncrementArmPresetPositionCommand.cpp index e03deeb..a1e48bd 100644 --- a/src/main/cpp/commands/arm/IncrementArmPresetPositionCommand.cpp +++ b/src/main/cpp/commands/arm/IncrementArmPresetPositionCommand.cpp @@ -19,7 +19,7 @@ IncrementArmPresetPositionCommand::IncrementArmPresetPositionCommand(std::shared } void IncrementArmPresetPositionCommand::Initialize() { - arm_->SetControlMode(Test); + } void IncrementArmPresetPositionCommand::Execute() { @@ -27,9 +27,7 @@ void IncrementArmPresetPositionCommand::Execute() { VOKC_CALL(arm_->IncrementRotation(increment_)); } - - - bool IncrementArmPresetPositionCommand::IsFinished() { +bool IncrementArmPresetPositionCommand::IsFinished() { bool at = false; OKC_CALL(arm_->AtExtendSetpoint(&at)); diff --git a/src/main/cpp/subsystems/Arm.cpp b/src/main/cpp/subsystems/Arm.cpp index d97eddd..ed411a4 100644 --- a/src/main/cpp/subsystems/Arm.cpp +++ b/src/main/cpp/subsystems/Arm.cpp @@ -74,7 +74,7 @@ bool Arm::SetDesiredState(TeamOKC::ArmState state) { } bool Arm::IncrementRotation(double increment) { - this->desired_state_.rotation += increment; + this->test_state_.rotation += increment; has_been_commanded_ = true; control_state_ = ROTATING; @@ -83,7 +83,7 @@ bool Arm::IncrementRotation(double increment) { } bool Arm::IncrementExtend(double increment) { - this->desired_state_.extension += increment; + this->test_state_.extension += increment; has_been_commanded_ = true; control_state_ = ROTATING; @@ -132,8 +132,8 @@ bool Arm::TestControl() { has_calibrated_ = true; // initialize the desired state to the state when we are calibrated - this->desired_state_.rotation = this->interface_->arm_duty_cycle_encoder; - this->desired_state_.extension = 1; // slightly not 0 just because + this->test_state_.rotation = this->interface_->arm_duty_cycle_encoder; + this->test_state_.extension = 1; // slightly not 0 just because } else { // otherwise, we haven't hit it yet, so set the motor to a small negative power until we do this->interface_->arm_extend_power = -0.1; @@ -151,19 +151,19 @@ bool Arm::TestControl() { } // limit extension - if (this->desired_state_.extension > extend_limit_) { - this->desired_state_.extension = extend_limit_; - } else if (this->desired_state_.extension < 0) { - this->desired_state_.extension = 0; + if (this->test_state_.extension > extend_limit_) { + this->test_state_.extension = extend_limit_; + } else if (this->test_state_.extension < 1) { + this->test_state_.extension = 1; } // set setpoints - this->arm_pid_->SetSetpoint(this->desired_state_.rotation); - this->inches_pid_->SetSetpoint(this->desired_state_.extension); + this->arm_pid_->SetSetpoint(this->test_state_.rotation); + this->inches_pid_->SetSetpoint(this->test_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; + double ff = TeamOKC::sign(this->test_state_.rotation) * arm_kF_ * this->test_state_.rotation * this->test_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); @@ -223,10 +223,10 @@ bool Arm::AutoControl() { // alright, we've made it thus far, so we need to get down to business and start moving if (control_state_ == ROTATING) { // bring the extension in whenever we rotate the arm, to reduce bounce - this->inches_pid_->SetSetpoint(0.5); + this->inches_pid_->SetSetpoint(1); // if we have brought the extension in - if (abs(1.0 - state_.extension) < 1.0) { + if (abs(state_.extension) < 2.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); @@ -261,7 +261,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) < 7.0) { + if (abs(this->desired_state_.rotation) < 2.0 && abs(this->state_.rotation) < 20) { // to prevent it from oscillating just set it to 0 this->interface_->arm_lift_power = 0.0; } else { @@ -276,7 +276,9 @@ bool Arm::AutoControl() { } void Arm::Periodic() { - if (ArmUI::nt_test_mode->GetBoolean(false)) { + // std::cout << mode_ << std::endl; + + if (ArmUI::nt_test_mode->GetBoolean(false)) { mode_ = Test; } else if (ArmUI::nt_manual_arm_mode->GetBoolean(false)) { mode_ = Manual; diff --git a/src/main/include/subsystems/Arm.h b/src/main/include/subsystems/Arm.h index 8455bf5..68da4cf 100644 --- a/src/main/include/subsystems/Arm.h +++ b/src/main/include/subsystems/Arm.h @@ -62,6 +62,7 @@ class Arm : public frc2::SubsystemBase { ArmControlState control_state_; TeamOKC::ArmState state_; TeamOKC::ArmState desired_state_; + TeamOKC::ArmState test_state_; // control flags bool has_been_commanded_ = false;