| #include "y2018/control_loops/superstructure/superstructure.h" |
| |
| #include <chrono> |
| |
| #include "aos/logging/logging.h" |
| #include "frc971/control_loops/control_loops_generated.h" |
| #include "frc971/control_loops/drivetrain/drivetrain_output_generated.h" |
| #include "y2018/constants.h" |
| #include "y2018/control_loops/superstructure/intake/intake.h" |
| #include "y2018/status_light_generated.h" |
| #include "y2018/vision/vision_generated.h" |
| |
| namespace y2018 { |
| namespace control_loops { |
| namespace superstructure { |
| |
| using ::aos::monotonic_clock; |
| |
| namespace chrono = ::std::chrono; |
| |
| namespace { |
| // The maximum voltage the intake roller will be allowed to use. |
| constexpr double kMaxIntakeRollerVoltage = 12.0; |
| } // namespace |
| |
| Superstructure::Superstructure(::aos::EventLoop *event_loop, |
| const ::std::string &name) |
| : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop, |
| name), |
| status_light_sender_( |
| event_loop->MakeSender<::y2018::StatusLight>("/superstructure")), |
| vision_status_fetcher_( |
| event_loop->MakeFetcher<::y2018::vision::VisionStatus>( |
| "/superstructure")), |
| drivetrain_output_fetcher_( |
| event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Output>( |
| "/drivetrain")), |
| intake_left_(constants::GetValues().left_intake.zeroing), |
| intake_right_(constants::GetValues().right_intake.zeroing) {} |
| |
| void Superstructure::RunIteration(const Goal *unsafe_goal, |
| const Position *position, |
| aos::Sender<Output>::Builder *output, |
| aos::Sender<Status>::Builder *status) { |
| const monotonic_clock::time_point monotonic_now = |
| event_loop()->monotonic_now(); |
| if (WasReset()) { |
| AOS_LOG(ERROR, "WPILib reset, restarting\n"); |
| intake_left_.Reset(); |
| intake_right_.Reset(); |
| arm_.Reset(); |
| } |
| |
| const double clipped_box_distance = |
| ::std::min(1.0, ::std::max(0.0, position->box_distance())); |
| |
| const double box_velocity = |
| (clipped_box_distance - last_box_distance_) / 0.005; |
| |
| constexpr double kFilteredBoxVelocityAlpha = 0.02; |
| filtered_box_velocity_ = |
| box_velocity * kFilteredBoxVelocityAlpha + |
| (1.0 - kFilteredBoxVelocityAlpha) * filtered_box_velocity_; |
| |
| constexpr double kCenteringAngleGain = 0.0; |
| const double left_intake_goal = |
| ::std::min(arm_.max_intake_override(), |
| (unsafe_goal == nullptr || !unsafe_goal->has_intake() |
| ? 0.0 |
| : unsafe_goal->intake()->left_intake_angle())) + |
| last_intake_center_error_ * kCenteringAngleGain; |
| const double right_intake_goal = |
| ::std::min(arm_.max_intake_override(), |
| (unsafe_goal == nullptr || !unsafe_goal->has_intake() |
| ? 0.0 |
| : unsafe_goal->intake()->right_intake_angle())) - |
| last_intake_center_error_ * kCenteringAngleGain; |
| |
| IntakeVoltageT left_intake_output; |
| flatbuffers::Offset<superstructure::IntakeSideStatus> left_status_offset = |
| intake_left_.Iterate( |
| unsafe_goal != nullptr ? &(left_intake_goal) : nullptr, |
| position->left_intake(), |
| output != nullptr ? &(left_intake_output) : nullptr, status->fbb()); |
| |
| IntakeVoltageT right_intake_output; |
| flatbuffers::Offset<superstructure::IntakeSideStatus> right_status_offset = |
| intake_right_.Iterate( |
| unsafe_goal != nullptr ? &(right_intake_goal) : nullptr, |
| position->right_intake(), |
| output != nullptr ? &(right_intake_output) : nullptr, status->fbb()); |
| |
| const double intake_center_error = |
| intake_right_.output_position() - intake_left_.output_position(); |
| last_intake_center_error_ = intake_center_error; |
| |
| const bool intake_clear_of_box = |
| intake_left_.clear_of_box() && intake_right_.clear_of_box(); |
| |
| bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw() : false; |
| if (unsafe_goal) { |
| if (unsafe_goal->open_threshold() != 0.0) { |
| if (arm_.current_node() != unsafe_goal->arm_goal_position() || |
| arm_.path_distance_to_go() > unsafe_goal->open_threshold()) { |
| open_claw = false; |
| } |
| } |
| } |
| |
| const uint32_t arm_goal_position = |
| unsafe_goal != nullptr ? unsafe_goal->arm_goal_position() : 0u; |
| |
| double voltage_proximal_output = 0.0; |
| double voltage_distal_output = 0.0; |
| bool release_arm_brake_output = false; |
| bool claw_grabbed_output = false; |
| flatbuffers::Offset<superstructure::ArmStatus> arm_status_offset = |
| arm_.Iterate( |
| monotonic_now, |
| unsafe_goal != nullptr ? &(arm_goal_position) : nullptr, |
| unsafe_goal != nullptr ? unsafe_goal->grab_box() : false, open_claw, |
| unsafe_goal != nullptr ? unsafe_goal->close_claw() : false, |
| position->arm(), position->claw_beambreak_triggered(), |
| position->box_back_beambreak_triggered(), intake_clear_of_box, |
| unsafe_goal != nullptr ? unsafe_goal->voltage_winch() > 1.0 : false, |
| unsafe_goal != nullptr ? unsafe_goal->trajectory_override() : false, |
| output != nullptr ? &voltage_proximal_output : nullptr, |
| output != nullptr ? &voltage_distal_output : nullptr, |
| output != nullptr ? &release_arm_brake_output : nullptr, |
| output != nullptr ? &claw_grabbed_output : nullptr, status->fbb()); |
| |
| |
| bool hook_release_output = false; |
| bool forks_release_output = false; |
| double voltage_winch_output = 0.0; |
| if (output) { |
| if (unsafe_goal) { |
| hook_release_output = unsafe_goal->hook_release(); |
| voltage_winch_output = unsafe_goal->voltage_winch(); |
| forks_release_output = unsafe_goal->deploy_fork(); |
| } |
| } |
| |
| Status::Builder status_builder = status->MakeBuilder<Status>(); |
| |
| status_builder.add_left_intake(left_status_offset); |
| status_builder.add_right_intake(right_status_offset); |
| status_builder.add_arm(arm_status_offset); |
| |
| status_builder.add_filtered_box_velocity(filtered_box_velocity_); |
| const bool estopped = |
| intake_left_.estopped() || intake_right_.estopped() || arm_.estopped(); |
| status_builder.add_estopped(estopped); |
| |
| status_builder.add_zeroed(intake_left_.zeroed() && intake_right_.zeroed() && |
| arm_.zeroed()); |
| |
| if (output && unsafe_goal) { |
| double roller_voltage = |
| ::std::max(-kMaxIntakeRollerVoltage, |
| ::std::min(unsafe_goal->intake()->roller_voltage(), |
| kMaxIntakeRollerVoltage)); |
| constexpr int kReverseTime = 14; |
| if (unsafe_goal->intake()->roller_voltage() < 0.0 || |
| unsafe_goal->disable_box_correct()) { |
| left_intake_output.voltage_rollers = roller_voltage; |
| right_intake_output.voltage_rollers = roller_voltage; |
| rotation_state_ = RotationState::NOT_ROTATING; |
| rotation_count_ = 0; |
| stuck_count_ = 0; |
| } else { |
| const bool stuck = position->box_distance() < 0.20 && |
| filtered_box_velocity_ > -0.05 && |
| !position->box_back_beambreak_triggered(); |
| // Make sure we don't declare ourselves re-stuck too quickly. We want to |
| // wait 400 ms before triggering the stuck condition again. |
| if (!stuck) { |
| last_unstuck_time_ = monotonic_now; |
| } |
| if (monotonic_now < last_stuck_time_ + chrono::milliseconds(400)) { |
| last_unstuck_time_ = monotonic_now; |
| } |
| |
| switch (rotation_state_) { |
| case RotationState::NOT_ROTATING: |
| if (stuck && |
| monotonic_now > last_stuck_time_ + chrono::milliseconds(400) && |
| monotonic_now > last_unstuck_time_ + chrono::milliseconds(100)) { |
| rotation_state_ = RotationState::STUCK; |
| ++stuck_count_; |
| last_stuck_time_ = monotonic_now; |
| } else if (position->left_intake()->beam_break()) { |
| rotation_state_ = RotationState::ROTATING_RIGHT; |
| rotation_count_ = kReverseTime; |
| break; |
| } else if (position->right_intake()->beam_break()) { |
| rotation_state_ = RotationState::ROTATING_LEFT; |
| rotation_count_ = kReverseTime; |
| break; |
| } else { |
| break; |
| } |
| case RotationState::STUCK: { |
| // Latch being stuck for 80 ms so we kick the box out far enough. |
| if (last_stuck_time_ + chrono::milliseconds(80) < monotonic_now) { |
| rotation_state_ = RotationState::NOT_ROTATING; |
| last_unstuck_time_ = monotonic_now; |
| } |
| } break; |
| case RotationState::ROTATING_LEFT: |
| if (position->right_intake()->beam_break()) { |
| rotation_count_ = kReverseTime; |
| } else { |
| --rotation_count_; |
| } |
| if (rotation_count_ == 0) { |
| rotation_state_ = RotationState::NOT_ROTATING; |
| } |
| break; |
| case RotationState::ROTATING_RIGHT: |
| if (position->left_intake()->beam_break()) { |
| rotation_count_ = kReverseTime; |
| } else { |
| --rotation_count_; |
| } |
| if (rotation_count_ == 0) { |
| rotation_state_ = RotationState::NOT_ROTATING; |
| } |
| break; |
| } |
| |
| constexpr double kHoldVoltage = 1.0; |
| constexpr double kStuckVoltage = 10.0; |
| |
| if (position->box_back_beambreak_triggered() && |
| roller_voltage > kHoldVoltage) { |
| roller_voltage = kHoldVoltage; |
| } |
| switch (rotation_state_) { |
| case RotationState::NOT_ROTATING: { |
| double centering_gain = 13.0; |
| if (stuck_count_ > 1) { |
| if ((stuck_count_ - 1) % 2 == 0) { |
| centering_gain = 0.0; |
| } |
| } |
| left_intake_output.voltage_rollers = |
| roller_voltage - intake_center_error * centering_gain; |
| right_intake_output.voltage_rollers = |
| roller_voltage + intake_center_error * centering_gain; |
| } break; |
| case RotationState::STUCK: { |
| if (roller_voltage > kHoldVoltage) { |
| left_intake_output.voltage_rollers = -kStuckVoltage; |
| right_intake_output.voltage_rollers = -kStuckVoltage; |
| } |
| } break; |
| case RotationState::ROTATING_LEFT: |
| if (position->left_intake()->beam_break()) { |
| left_intake_output.voltage_rollers = -roller_voltage * 0.9; |
| } else { |
| left_intake_output.voltage_rollers = -roller_voltage * 0.6; |
| } |
| right_intake_output.voltage_rollers = roller_voltage; |
| break; |
| case RotationState::ROTATING_RIGHT: |
| left_intake_output.voltage_rollers = roller_voltage; |
| if (position->right_intake()->beam_break()) { |
| right_intake_output.voltage_rollers = -roller_voltage * 0.9; |
| } else { |
| right_intake_output.voltage_rollers = -roller_voltage * 0.6; |
| } |
| break; |
| } |
| } |
| } else { |
| rotation_state_ = RotationState::NOT_ROTATING; |
| rotation_count_ = 0; |
| stuck_count_ = 0; |
| } |
| status_builder.add_rotation_state(static_cast<uint32_t>(rotation_state_)); |
| |
| drivetrain_output_fetcher_.Fetch(); |
| |
| vision_status_fetcher_.Fetch(); |
| if (estopped) { |
| SendColors(0.5, 0.0, 0.0); |
| } else if (!vision_status_fetcher_.get() || |
| monotonic_now > |
| vision_status_fetcher_.context().monotonic_sent_time + |
| chrono::seconds(1)) { |
| SendColors(0.5, 0.5, 0.0); |
| } else if (rotation_state_ == RotationState::ROTATING_LEFT || |
| rotation_state_ == RotationState::ROTATING_RIGHT) { |
| SendColors(0.5, 0.20, 0.0); |
| } else if (rotation_state_ == RotationState::STUCK) { |
| SendColors(0.5, 0.0, 0.5); |
| } else if (position->box_back_beambreak_triggered()) { |
| SendColors(0.0, 0.0, 0.5); |
| } else if (position->box_distance() < 0.2) { |
| SendColors(0.0, 0.5, 0.0); |
| } else if (drivetrain_output_fetcher_.get() && |
| ::std::max( |
| ::std::abs(drivetrain_output_fetcher_->left_voltage()), |
| ::std::abs(drivetrain_output_fetcher_->right_voltage())) > |
| 11.5) { |
| SendColors(0.5, 0.0, 0.5); |
| } else { |
| SendColors(0.0, 0.0, 0.0); |
| } |
| |
| last_box_distance_ = clipped_box_distance; |
| |
| if (output) { |
| flatbuffers::Offset<IntakeVoltage> left_intake_offset = |
| IntakeVoltage::Pack(*output->fbb(), &left_intake_output); |
| flatbuffers::Offset<IntakeVoltage> right_intake_offset = |
| IntakeVoltage::Pack(*output->fbb(), &right_intake_output); |
| |
| Output::Builder output_builder = output->MakeBuilder<Output>(); |
| output_builder.add_left_intake(left_intake_offset); |
| output_builder.add_right_intake(right_intake_offset); |
| output_builder.add_voltage_proximal(voltage_proximal_output); |
| output_builder.add_voltage_distal(voltage_distal_output); |
| output_builder.add_release_arm_brake(release_arm_brake_output); |
| output_builder.add_claw_grabbed(claw_grabbed_output); |
| |
| output_builder.add_hook_release(hook_release_output); |
| output_builder.add_forks_release(forks_release_output); |
| output_builder.add_voltage_winch(voltage_winch_output); |
| |
| output->Send(output_builder.Finish()); |
| } |
| |
| status->Send(status_builder.Finish()); |
| } |
| |
| void Superstructure::SendColors(float red, float green, float blue) { |
| auto builder = status_light_sender_.MakeBuilder(); |
| StatusLight::Builder status_light_builder = |
| builder.MakeBuilder<StatusLight>(); |
| |
| status_light_builder.add_red(red); |
| status_light_builder.add_green(green); |
| status_light_builder.add_blue(blue); |
| |
| if (!builder.Send(status_light_builder.Finish())) { |
| AOS_LOG(ERROR, "Failed to send lights.\n"); |
| } |
| } |
| |
| } // namespace superstructure |
| } // namespace control_loops |
| } // namespace y2018 |