| #include "y2018/control_loops/superstructure/superstructure.h" |
| |
| #include <chrono> |
| |
| #include "aos/controls/control_loops.q.h" |
| #include "aos/logging/logging.h" |
| #include "frc971/control_loops/control_loops.q.h" |
| #include "frc971/control_loops/drivetrain/drivetrain.q.h" |
| #include "y2018/constants.h" |
| #include "y2018/control_loops/superstructure/intake/intake.h" |
| #include "y2018/status_light.q.h" |
| #include "y2018/vision/vision.q.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<control_loops::SuperstructureQueue>(event_loop, |
| name), |
| status_light_sender_( |
| event_loop->MakeSender<::y2018::StatusLight>(".y2018.status_light")), |
| vision_status_fetcher_( |
| event_loop->MakeFetcher<::y2018::vision::VisionStatus>( |
| ".y2018.vision.vision_status")), |
| drivetrain_output_fetcher_( |
| event_loop |
| ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>( |
| ".frc971.control_loops.drivetrain_queue.output")), |
| intake_left_(constants::GetValues().left_intake.zeroing), |
| intake_right_(constants::GetValues().right_intake.zeroing) {} |
| |
| void Superstructure::RunIteration( |
| const control_loops::SuperstructureQueue::Goal *unsafe_goal, |
| const control_loops::SuperstructureQueue::Position *position, |
| control_loops::SuperstructureQueue::Output *output, |
| control_loops::SuperstructureQueue::Status *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_; |
| status->filtered_box_velocity = filtered_box_velocity_; |
| |
| constexpr double kCenteringAngleGain = 0.0; |
| const double left_intake_goal = |
| ::std::min( |
| arm_.max_intake_override(), |
| (unsafe_goal == nullptr ? 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 ? 0.0 |
| : unsafe_goal->intake.right_intake_angle)) - |
| last_intake_center_error_ * kCenteringAngleGain; |
| |
| intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr, |
| &(position->left_intake), |
| output != nullptr ? &(output->left_intake) : nullptr, |
| &(status->left_intake)); |
| |
| intake_right_.Iterate(unsafe_goal != nullptr ? &(right_intake_goal) : nullptr, |
| &(position->right_intake), |
| output != nullptr ? &(output->right_intake) : nullptr, |
| &(status->right_intake)); |
| |
| 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; |
| } |
| } |
| } |
| arm_.Iterate( |
| monotonic_now, |
| unsafe_goal != nullptr ? &(unsafe_goal->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 ? &(output->voltage_proximal) : nullptr, |
| output != nullptr ? &(output->voltage_distal) : nullptr, |
| output != nullptr ? &(output->release_arm_brake) : nullptr, |
| output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm)); |
| |
| if (output) { |
| if (unsafe_goal) { |
| output->hook_release = unsafe_goal->hook_release; |
| output->voltage_winch = unsafe_goal->voltage_winch; |
| output->forks_release = unsafe_goal->deploy_fork; |
| } else { |
| output->voltage_winch = 0.0; |
| output->hook_release = false; |
| output->forks_release = false; |
| } |
| } |
| |
| status->estopped = status->left_intake.estopped || |
| status->right_intake.estopped || status->arm.estopped; |
| |
| status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed && |
| status->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) { |
| output->left_intake.voltage_rollers = roller_voltage; |
| output->right_intake.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; |
| } |
| } |
| output->left_intake.voltage_rollers = |
| roller_voltage - intake_center_error * centering_gain; |
| output->right_intake.voltage_rollers = |
| roller_voltage + intake_center_error * centering_gain; |
| } break; |
| case RotationState::STUCK: { |
| if (roller_voltage > kHoldVoltage) { |
| output->left_intake.voltage_rollers = -kStuckVoltage; |
| output->right_intake.voltage_rollers = -kStuckVoltage; |
| } |
| } break; |
| case RotationState::ROTATING_LEFT: |
| if (position->left_intake.beam_break) { |
| output->left_intake.voltage_rollers = -roller_voltage * 0.9; |
| } else { |
| output->left_intake.voltage_rollers = -roller_voltage * 0.6; |
| } |
| output->right_intake.voltage_rollers = roller_voltage; |
| break; |
| case RotationState::ROTATING_RIGHT: |
| output->left_intake.voltage_rollers = roller_voltage; |
| if (position->right_intake.beam_break) { |
| output->right_intake.voltage_rollers = -roller_voltage * 0.9; |
| } else { |
| output->right_intake.voltage_rollers = -roller_voltage * 0.6; |
| } |
| break; |
| } |
| } |
| } else { |
| rotation_state_ = RotationState::NOT_ROTATING; |
| rotation_count_ = 0; |
| stuck_count_ = 0; |
| } |
| status->rotation_state = static_cast<uint32_t>(rotation_state_); |
| |
| drivetrain_output_fetcher_.Fetch(); |
| |
| vision_status_fetcher_.Fetch(); |
| if (status->estopped) { |
| SendColors(0.5, 0.0, 0.0); |
| } else if (!vision_status_fetcher_.get() || |
| monotonic_now > |
| vision_status_fetcher_->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; |
| } |
| |
| void Superstructure::SendColors(float red, float green, float blue) { |
| auto new_status_light = status_light_sender_.MakeMessage(); |
| new_status_light->red = red; |
| new_status_light->green = green; |
| new_status_light->blue = blue; |
| |
| if (!new_status_light.Send()) { |
| AOS_LOG(ERROR, "Failed to send lights.\n"); |
| } |
| } |
| |
| } // namespace superstructure |
| } // namespace control_loops |
| } // namespace y2018 |