| #include "y2016/control_loops/superstructure/superstructure.h" |
| |
| #include "aos/commonmath.h" |
| #include "aos/logging/logging.h" |
| |
| #include "y2016/control_loops/superstructure/integral_arm_plant.h" |
| #include "y2016/control_loops/superstructure/integral_intake_plant.h" |
| #include "y2016/control_loops/superstructure/superstructure_controls.h" |
| #include "y2016/queues/ball_detector_generated.h" |
| |
| #include "y2016/constants.h" |
| |
| namespace y2016 { |
| namespace control_loops { |
| namespace superstructure { |
| |
| namespace { |
| // The maximum voltage the intake roller will be allowed to use. |
| constexpr float kMaxIntakeTopVoltage = 12.0; |
| constexpr float kMaxIntakeBottomVoltage = 12.0; |
| constexpr float kMaxClimberVoltage = 12.0; |
| |
| // Aliases to reduce typing. |
| constexpr double kIntakeEncoderIndexDifference = |
| constants::Values::kIntakeEncoderIndexDifference; |
| constexpr double kWristEncoderIndexDifference = |
| constants::Values::kWristEncoderIndexDifference; |
| constexpr double kShoulderEncoderIndexDifference = |
| constants::Values::kShoulderEncoderIndexDifference; |
| } // namespace |
| |
| // ///// CollisionAvoidance ///// |
| |
| void CollisionAvoidance::UpdateGoal(double shoulder_angle_goal, |
| double wrist_angle_goal, |
| double intake_angle_goal) { |
| const double original_shoulder_angle_goal = shoulder_angle_goal; |
| const double original_intake_angle_goal = intake_angle_goal; |
| const double original_wrist_angle_goal = wrist_angle_goal; |
| |
| double shoulder_angle = arm_->shoulder_angle(); |
| double wrist_angle = arm_->wrist_angle(); |
| double intake_angle = intake_->position(); |
| |
| // TODO(phil): This may need tuning to account for bounciness in the limbs or |
| // some other thing that I haven't thought of. At the very least, |
| // incorporating a small safety margin makes writing test cases much easier |
| // since you can directly compare statuses against the constants in the |
| // CollisionAvoidance class. |
| constexpr double kSafetyMargin = 0.03; // radians |
| |
| // Avoid colliding the shooter with the frame. |
| // If the shoulder is below a certain angle or we want to move it below |
| // that angle, then the shooter has to stay level to the ground. Otherwise, |
| // it will crash into the frame. |
| if (intake_angle < kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) { |
| if (shoulder_angle < kMinShoulderAngleForHorizontalShooter || |
| original_shoulder_angle_goal < kMinShoulderAngleForHorizontalShooter) { |
| wrist_angle_goal = 0.0; |
| } else if (shoulder_angle < kMinShoulderAngleForIntakeInterference || |
| original_shoulder_angle_goal < |
| kMinShoulderAngleForIntakeInterference) { |
| wrist_angle_goal = |
| aos::Clip(original_wrist_angle_goal, |
| kMinWristAngleForMovingByIntake + kSafetyMargin, |
| kMaxWristAngleForMovingByIntake - kSafetyMargin); |
| } |
| } else { |
| if (shoulder_angle < kMinShoulderAngleForIntakeUpInterference || |
| original_shoulder_angle_goal < |
| kMinShoulderAngleForIntakeUpInterference) { |
| wrist_angle_goal = 0.0; |
| } |
| } |
| |
| if (shoulder_angle < kMinShoulderAngleForIntakeUpInterference || |
| original_shoulder_angle_goal < kMinShoulderAngleForIntakeUpInterference) { |
| // Make sure that we don't move the shoulder below a certain angle until |
| // the wrist is level with the ground. |
| if (intake_angle < kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) { |
| if (wrist_angle > kMaxWristAngleForMovingByIntake || |
| wrist_angle < kMinWristAngleForMovingByIntake) { |
| shoulder_angle_goal = |
| ::std::max(original_shoulder_angle_goal, |
| kMinShoulderAngleForIntakeInterference + kSafetyMargin); |
| } |
| } else { |
| if (wrist_angle > kMaxWristAngleForMovingByIntake || |
| wrist_angle < kMinWristAngleForMovingByIntake) { |
| shoulder_angle_goal = ::std::max( |
| original_shoulder_angle_goal, |
| kMinShoulderAngleForIntakeUpInterference + kSafetyMargin); |
| } |
| } |
| if (::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) { |
| shoulder_angle_goal = |
| ::std::max(shoulder_angle_goal, |
| kMinShoulderAngleForHorizontalShooter + kSafetyMargin); |
| } |
| } |
| |
| // Is the arm where it could interfere with the intake right now? |
| bool shoulder_is_in_danger = |
| (shoulder_angle < kMinShoulderAngleForIntakeUpInterference && |
| shoulder_angle > kMaxShoulderAngleUntilSafeIntakeStowing); |
| |
| // Is the arm moving into collision zone from above? |
| bool shoulder_moving_into_danger_from_above = |
| (shoulder_angle >= kMinShoulderAngleForIntakeUpInterference && |
| original_shoulder_angle_goal <= |
| kMinShoulderAngleForIntakeUpInterference); |
| |
| // Is the arm moving into collision zone from below? |
| bool shoulder_moving_into_danger_from_below = |
| (shoulder_angle <= kMaxShoulderAngleUntilSafeIntakeStowing && |
| original_shoulder_angle_goal >= kMaxShoulderAngleUntilSafeIntakeStowing); |
| |
| // Avoid colliding the arm with the intake. |
| if (shoulder_is_in_danger || shoulder_moving_into_danger_from_above || |
| shoulder_moving_into_danger_from_below) { |
| // If the arm could collide with the intake, we make sure to move the |
| // intake out of the way. The arm has priority. |
| intake_angle_goal = |
| ::std::min(original_intake_angle_goal, |
| kMaxIntakeAngleBeforeArmInterference - kSafetyMargin); |
| |
| // Don't let the shoulder move into the collision area until the intake is |
| // out of the way. |
| if (intake_angle > kMaxIntakeAngleBeforeArmInterference) { |
| const double kHalfwayPointBetweenSafeZones = |
| (kMinShoulderAngleForIntakeInterference + |
| kMaxShoulderAngleUntilSafeIntakeStowing) / |
| 2.0; |
| |
| if (shoulder_angle >= kHalfwayPointBetweenSafeZones) { |
| // The shoulder is closer to being above the collision area. Move it up |
| // there. |
| if (intake_angle < |
| kMaxIntakeAngleBeforeArmInterference + kSafetyMargin) { |
| shoulder_angle_goal = ::std::max( |
| original_shoulder_angle_goal, |
| kMinShoulderAngleForIntakeInterference + kSafetyMargin); |
| } else { |
| shoulder_angle_goal = ::std::max( |
| original_shoulder_angle_goal, |
| kMinShoulderAngleForIntakeUpInterference + kSafetyMargin); |
| } |
| } else { |
| // The shoulder is closer to being below the collision zone (i.e. in |
| // stowing/intake position), keep it there for now. |
| shoulder_angle_goal = |
| ::std::min(original_shoulder_angle_goal, |
| kMaxShoulderAngleUntilSafeIntakeStowing - kSafetyMargin); |
| } |
| } |
| } |
| |
| // Send the possibly adjusted goals to the components. |
| arm_->set_unprofiled_goal(shoulder_angle_goal, wrist_angle_goal); |
| intake_->set_unprofiled_goal(intake_angle_goal); |
| } |
| |
| bool CollisionAvoidance::collided() const { |
| return collided_with_given_angles(arm_->shoulder_angle(), arm_->wrist_angle(), |
| intake_->position()); |
| } |
| |
| bool CollisionAvoidance::collided_with_given_angles(double shoulder_angle, |
| double wrist_angle, |
| double intake_angle) { |
| // The arm and the intake must not hit. |
| if (shoulder_angle >= |
| CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing && |
| shoulder_angle <= |
| CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference && |
| intake_angle > CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference) { |
| AOS_LOG(DEBUG, "Collided: Intake %f > %f, and shoulder %f < %f < %f.\n", |
| intake_angle, |
| CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference, |
| CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing, |
| shoulder_angle, |
| CollisionAvoidance::kMinShoulderAngleForIntakeUpInterference); |
| return true; |
| } |
| |
| if (shoulder_angle >= |
| CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing && |
| shoulder_angle <= |
| CollisionAvoidance::kMinShoulderAngleForIntakeInterference && |
| intake_angle < CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference && |
| intake_angle > Superstructure::kIntakeLowerClear && |
| (wrist_angle > CollisionAvoidance::kMaxWristAngleForMovingByIntake || |
| wrist_angle < CollisionAvoidance::kMinWristAngleForMovingByIntake)) { |
| AOS_LOG( |
| DEBUG, |
| "Collided: Intake %f < %f < %f, shoulder %f < %f < %f, and %f < %f < " |
| "%f.\n", |
| Superstructure::kIntakeLowerClear, intake_angle, |
| CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference, |
| CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing, |
| shoulder_angle, |
| CollisionAvoidance::kMinShoulderAngleForIntakeInterference, |
| CollisionAvoidance::kMinWristAngleForMovingByIntake, wrist_angle, |
| CollisionAvoidance::kMaxWristAngleForMovingByIntake); |
| return true; |
| } |
| |
| // The wrist must go back to zero when the shoulder is moving the arm into |
| // a stowed/intaking position. |
| if (shoulder_angle < |
| CollisionAvoidance::kMinShoulderAngleForHorizontalShooter && |
| ::std::abs(wrist_angle) > kMaxWristAngleForSafeArmStowing) { |
| AOS_LOG(DEBUG, "Collided: Shoulder %f < %f and wrist |%f| > %f.\n", |
| shoulder_angle, |
| CollisionAvoidance::kMinShoulderAngleForHorizontalShooter, |
| wrist_angle, kMaxWristAngleForSafeArmStowing); |
| return true; |
| } |
| |
| return false; |
| } |
| |
| constexpr double CollisionAvoidance::kMinShoulderAngleForHorizontalShooter; |
| constexpr double CollisionAvoidance::kMinShoulderAngleForIntakeInterference; |
| constexpr double CollisionAvoidance::kMaxIntakeAngleBeforeArmInterference; |
| constexpr double CollisionAvoidance::kMaxWristAngleForSafeArmStowing; |
| constexpr double CollisionAvoidance::kMaxShoulderAngleUntilSafeIntakeStowing; |
| |
| Superstructure::Superstructure(::aos::EventLoop *event_loop, |
| const ::std::string &name) |
| : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop, |
| name), |
| ball_detector_fetcher_( |
| event_loop->MakeFetcher<::y2016::sensors::BallDetector>( |
| "/superstructure")), |
| collision_avoidance_(&intake_, &arm_) {} |
| |
| bool Superstructure::IsArmNear(double shoulder_tolerance, |
| double wrist_tolerance) { |
| return ((arm_.unprofiled_goal() - arm_.X_hat()) |
| .block<2, 1>(0, 0) |
| .lpNorm<Eigen::Infinity>() < shoulder_tolerance) && |
| ((arm_.unprofiled_goal() - arm_.X_hat()) |
| .block<4, 1>(0, 0) |
| .lpNorm<Eigen::Infinity>() < wrist_tolerance) && |
| ((arm_.unprofiled_goal() - arm_.goal()) |
| .block<4, 1>(0, 0) |
| .lpNorm<Eigen::Infinity>() < 1e-6); |
| } |
| |
| bool Superstructure::IsArmNear(double tolerance) { |
| return ((arm_.unprofiled_goal() - arm_.X_hat()) |
| .block<4, 1>(0, 0) |
| .lpNorm<Eigen::Infinity>() < tolerance) && |
| ((arm_.unprofiled_goal() - arm_.goal()) |
| .block<4, 1>(0, 0) |
| .lpNorm<Eigen::Infinity>() < 1e-6); |
| } |
| |
| bool Superstructure::IsIntakeNear(double tolerance) { |
| return ((intake_.unprofiled_goal() - intake_.X_hat()) |
| .block<2, 1>(0, 0) |
| .lpNorm<Eigen::Infinity>() < tolerance); |
| } |
| |
| double Superstructure::MoveButKeepAbove(double reference_angle, |
| double current_angle, |
| double move_distance) { |
| return -MoveButKeepBelow(-reference_angle, -current_angle, -move_distance); |
| } |
| |
| double Superstructure::MoveButKeepBelow(double reference_angle, |
| double current_angle, |
| double move_distance) { |
| // There are 3 interesting places to move to. |
| const double small_negative_move = current_angle - move_distance; |
| const double small_positive_move = current_angle + move_distance; |
| // And the reference angle. |
| |
| // Move the the highest one that is below reference_angle. |
| if (small_negative_move > reference_angle) { |
| return reference_angle; |
| } else if (small_positive_move > reference_angle) { |
| return small_negative_move; |
| } else { |
| return small_positive_move; |
| } |
| } |
| |
| void Superstructure::RunIteration( |
| const Goal *unsafe_goal, |
| const Position *position, |
| aos::Sender<Output>::Builder *output, |
| aos::Sender<Status>::Builder *status) { |
| const State state_before_switch = state_; |
| if (WasReset()) { |
| AOS_LOG(ERROR, "WPILib reset, restarting\n"); |
| arm_.Reset(); |
| intake_.Reset(); |
| state_ = UNINITIALIZED; |
| } |
| |
| // Bool to track if we should turn the motors on or not. |
| bool disable = output == nullptr; |
| |
| arm_.Correct(position->shoulder(), position->wrist()); |
| intake_.Correct(*position->intake()); |
| |
| // There are 2 main zeroing paths, HIGH_ARM_ZERO and LOW_ARM_ZERO. |
| // |
| // HIGH_ARM_ZERO works by lifting the arm all the way up so it is clear, |
| // moving the shooter to be horizontal, moving the intake out, and then moving |
| // the arm back down. |
| // |
| // LOW_ARM_ZERO works by moving the intake out of the way, lifting the arm up, |
| // leveling the shooter, and then moving back down. |
| |
| if (arm_.error() || intake_.error()) { |
| state_ = ESTOP; |
| } |
| |
| const bool is_collided = collided(); |
| switch (state_) { |
| case UNINITIALIZED: |
| // Wait in the uninitialized state until both the arm and intake are |
| // initialized. |
| AOS_LOG(DEBUG, "Uninitialized, waiting for intake and arm\n"); |
| if (arm_.initialized() && intake_.initialized()) { |
| state_ = DISABLED_INITIALIZED; |
| } |
| disable = true; |
| break; |
| |
| case DISABLED_INITIALIZED: |
| // Wait here until we are either fully zeroed while disabled, or we become |
| // enabled. At that point, figure out if we should HIGH_ARM_ZERO or |
| // LOW_ARM_ZERO. |
| if (disable) { |
| if (arm_.zeroed() && intake_.zeroed()) { |
| state_ = SLOW_RUNNING; |
| } |
| } else { |
| if (arm_.shoulder_angle() >= kShoulderMiddleAngle) { |
| state_ = HIGH_ARM_ZERO_LIFT_ARM; |
| } else { |
| state_ = LOW_ARM_ZERO_LOWER_INTAKE; |
| } |
| } |
| |
| // Set the goals to where we are now so when we start back up, we don't |
| // jump. |
| intake_.ForceGoal(intake_.position()); |
| arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle()); |
| // Set up the profile to be the zeroing profile. |
| intake_.AdjustProfile(0.5, 10); |
| arm_.AdjustProfile(0.5, 10, 0.5, 10); |
| |
| // We are not ready to start doing anything yet. |
| disable = true; |
| break; |
| |
| case HIGH_ARM_ZERO_LIFT_ARM: |
| if (disable) { |
| state_ = DISABLED_INITIALIZED; |
| } else { |
| // Raise the shoulder up out of the way. |
| arm_.set_unprofiled_goal(kShoulderUpAngle, arm_.wrist_angle()); |
| if (IsArmNear(kLooseTolerance)) { |
| // Close enough, start the next move. |
| state_ = HIGH_ARM_ZERO_LEVEL_SHOOTER; |
| } |
| } |
| break; |
| |
| case HIGH_ARM_ZERO_LEVEL_SHOOTER: |
| if (disable) { |
| state_ = DISABLED_INITIALIZED; |
| } else { |
| // Move the shooter to be level. |
| arm_.set_unprofiled_goal(kShoulderUpAngle, 0.0); |
| |
| if (IsArmNear(kLooseTolerance)) { |
| // Close enough, start the next move. |
| state_ = HIGH_ARM_ZERO_MOVE_INTAKE_OUT; |
| } |
| } |
| break; |
| |
| case HIGH_ARM_ZERO_MOVE_INTAKE_OUT: |
| if (disable) { |
| state_ = DISABLED_INITIALIZED; |
| } else { |
| // If we were just asked to move the intake, make sure it moves far |
| // enough. |
| if (last_state_ != HIGH_ARM_ZERO_MOVE_INTAKE_OUT) { |
| intake_.set_unprofiled_goal( |
| MoveButKeepBelow(kIntakeUpperClear, intake_.position(), |
| kIntakeEncoderIndexDifference * 2.5)); |
| } |
| |
| if (IsIntakeNear(kLooseTolerance)) { |
| // Close enough, start the next move. |
| state_ = HIGH_ARM_ZERO_LOWER_ARM; |
| } |
| } |
| break; |
| |
| case HIGH_ARM_ZERO_LOWER_ARM: |
| if (disable) { |
| state_ = DISABLED_INITIALIZED; |
| } else { |
| // Land the shooter in the belly-pan. It should be zeroed by the time |
| // it gets there. If not, just estop. |
| arm_.set_unprofiled_goal(kShoulderLanded, 0.0); |
| if (arm_.zeroed() && intake_.zeroed()) { |
| state_ = RUNNING; |
| } else if (IsArmNear(kLooseTolerance)) { |
| AOS_LOG(ERROR, |
| "Failed to zero while executing the HIGH_ARM_ZERO sequence. " |
| "Arm: %d Intake %d\n", |
| arm_.zeroed(), intake_.zeroed()); |
| state_ = ESTOP; |
| } |
| } |
| break; |
| |
| case LOW_ARM_ZERO_LOWER_INTAKE: |
| if (disable) { |
| state_ = DISABLED_INITIALIZED; |
| } else { |
| // Move the intake down out of the way of the arm. Make sure to move it |
| // far enough to zero. |
| if (last_state_ != LOW_ARM_ZERO_LOWER_INTAKE) { |
| intake_.set_unprofiled_goal( |
| MoveButKeepBelow(kIntakeLowerClear, intake_.position(), |
| kIntakeEncoderIndexDifference * 2.5)); |
| } |
| if (IsIntakeNear(kLooseTolerance)) { |
| if (::std::abs(arm_.wrist_angle()) < kWristAlmostLevel) { |
| state_ = LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER; |
| } else { |
| state_ = LOW_ARM_ZERO_LIFT_SHOULDER; |
| } |
| } |
| } |
| break; |
| |
| case LOW_ARM_ZERO_MAYBE_LEVEL_SHOOTER: |
| if (disable) { |
| state_ = DISABLED_INITIALIZED; |
| } else { |
| // If we are supposed to level the shooter, set it to level, and wait |
| // until it is very close to level. |
| arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), 0.0); |
| if (IsArmNear(kLooseTolerance, kTightTolerance)) { |
| state_ = LOW_ARM_ZERO_LIFT_SHOULDER; |
| } |
| } |
| break; |
| |
| case LOW_ARM_ZERO_LIFT_SHOULDER: |
| if (disable) { |
| state_ = DISABLED_INITIALIZED; |
| } else { |
| // Decide where to move to. We need to move far enough to see an index |
| // pulse, but must also get high enough that we can safely level the |
| // shooter. |
| if (last_state_ != LOW_ARM_ZERO_LIFT_SHOULDER) { |
| arm_.set_unprofiled_goal( |
| MoveButKeepAbove(kShoulderWristClearAngle, arm_.shoulder_angle(), |
| ::std::max(kWristEncoderIndexDifference, |
| kShoulderEncoderIndexDifference) * |
| 2.5), |
| arm_.unprofiled_goal(2, 0)); |
| } |
| |
| // If we're about to ask the wrist to go past one of its limits, then |
| // move the goal so it will be just at the limit when we finish lifting |
| // the shoulder. If it wasn't intersecting something before, this can't |
| // cause it to crash into anything. |
| const double ungrounded_wrist = arm_.goal(2, 0) - arm_.goal(0, 0); |
| const double unprofiled_ungrounded_wrist = |
| arm_.unprofiled_goal(2, 0) - arm_.unprofiled_goal(0, 0); |
| if (unprofiled_ungrounded_wrist > |
| constants::Values::kWristRange.upper && |
| ungrounded_wrist > |
| constants::Values::kWristRange.upper - kWristAlmostLevel) { |
| arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), |
| constants::Values::kWristRange.upper + |
| arm_.unprofiled_goal(0, 0)); |
| } else if (unprofiled_ungrounded_wrist < |
| constants::Values::kWristRange.lower && |
| ungrounded_wrist < constants::Values::kWristRange.lower + |
| kWristAlmostLevel) { |
| arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), |
| constants::Values::kWristRange.lower + |
| arm_.unprofiled_goal(0, 0)); |
| } |
| |
| // Wait until we are level and then go for it. |
| if (IsArmNear(kLooseTolerance)) { |
| state_ = LOW_ARM_ZERO_LEVEL_SHOOTER; |
| } |
| } |
| break; |
| |
| case LOW_ARM_ZERO_LEVEL_SHOOTER: |
| if (disable) { |
| state_ = DISABLED_INITIALIZED; |
| } else { |
| // Move the shooter level (and keep the same height). We don't want to |
| // got to RUNNING until we are completely level so that we don't |
| // give control back in a weird case where we might crash. |
| arm_.set_unprofiled_goal(arm_.unprofiled_goal(0, 0), 0.0); |
| if (IsArmNear(kLooseTolerance)) { |
| if (arm_.zeroed() && intake_.zeroed()) { |
| state_ = RUNNING; |
| } else { |
| AOS_LOG(ERROR, |
| "Failed to zero while executing the LOW_ARM_ZERO sequence. " |
| "Arm: %d Intake %d\n", |
| arm_.zeroed(), intake_.zeroed()); |
| state_ = ESTOP; |
| } |
| } |
| } |
| break; |
| |
| // These 4 cases are very similar. |
| case SLOW_RUNNING: |
| case RUNNING: |
| case LANDING_SLOW_RUNNING: |
| case LANDING_RUNNING: { |
| if (disable) { |
| // If we are disabled, go to slow running (or landing slow running) if |
| // we are collided. |
| if (is_collided) { |
| if (state_ == RUNNING) { |
| state_ = SLOW_RUNNING; |
| } else if (state_ == LANDING_RUNNING) { |
| state_ = LANDING_SLOW_RUNNING; |
| } |
| } |
| |
| // Reset the profile to the current position so it moves well from here. |
| intake_.ForceGoal(intake_.position()); |
| arm_.ForceGoal(arm_.shoulder_angle(), arm_.wrist_angle()); |
| } else { |
| // If we are in slow_running and are no longer collided, let 'er rip. |
| if (state_ == SLOW_RUNNING) { |
| if (!is_collided) { |
| state_ = RUNNING; |
| } |
| } else if (state_ == LANDING_SLOW_RUNNING) { |
| if (!is_collided) { |
| state_ = LANDING_RUNNING; |
| } |
| } |
| } |
| |
| double requested_shoulder = constants::Values::kShoulderRange.lower; |
| double requested_wrist = 0.0; |
| double requested_intake = M_PI / 2.0; |
| |
| if (unsafe_goal) { |
| // If we are in one of the landing states, limit the accelerations and |
| // velocities to land cleanly. |
| if (state_ == LANDING_SLOW_RUNNING || state_ == LANDING_RUNNING) { |
| arm_.AdjustProfile(0.5, // Shoulder Velocity |
| 4.0, // Shoulder acceleration, |
| 4.0, // Wrist velocity |
| 10.0); // Wrist acceleration. |
| intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(), |
| unsafe_goal->max_angular_acceleration_intake()); |
| |
| requested_shoulder = |
| ::std::max(unsafe_goal->angle_shoulder(), |
| constants::Values::kShoulderRange.lower); |
| requested_wrist = 0.0; |
| requested_intake = unsafe_goal->angle_intake(); |
| // Transition to landing once the profile is close to finished for the |
| // shoulder. |
| if (arm_.goal(0, 0) > kShoulderTransitionToLanded + 1e-4 || |
| arm_.unprofiled_goal(0, 0) > kShoulderTransitionToLanded + 1e-4) { |
| if (state_ == LANDING_RUNNING) { |
| state_ = RUNNING; |
| } else { |
| state_ = SLOW_RUNNING; |
| } |
| } |
| } else { |
| // Otherwise, give the user what he asked for. |
| arm_.AdjustProfile(unsafe_goal->max_angular_velocity_shoulder(), |
| unsafe_goal->max_angular_acceleration_shoulder(), |
| unsafe_goal->max_angular_velocity_wrist(), |
| unsafe_goal->max_angular_acceleration_wrist()); |
| intake_.AdjustProfile(unsafe_goal->max_angular_velocity_intake(), |
| unsafe_goal->max_angular_acceleration_intake()); |
| |
| // Except, don't let the shoulder go all the way down. |
| requested_shoulder = ::std::max(unsafe_goal->angle_shoulder(), |
| kShoulderTransitionToLanded); |
| requested_wrist = unsafe_goal->angle_wrist(); |
| requested_intake = unsafe_goal->angle_intake(); |
| |
| // Transition to landing once the profile is close to finished for the |
| // shoulder. |
| if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4 && |
| arm_.unprofiled_goal(0, 0) <= |
| kShoulderTransitionToLanded + 1e-4) { |
| if (state_ == RUNNING) { |
| state_ = LANDING_RUNNING; |
| } else { |
| state_ = LANDING_SLOW_RUNNING; |
| } |
| } |
| } |
| } |
| |
| // Push the request out to hardware! |
| collision_avoidance_.UpdateGoal(requested_shoulder, requested_wrist, |
| requested_intake); |
| |
| // ESTOP if we hit the hard limits. |
| if ((arm_.CheckHardLimits() || intake_.CheckHardLimits()) && output) { |
| state_ = ESTOP; |
| } |
| } break; |
| |
| case ESTOP: |
| AOS_LOG(ERROR, "Estop\n"); |
| disable = true; |
| break; |
| } |
| |
| // Set the voltage limits. |
| const double max_voltage = (state_ == RUNNING || state_ == LANDING_RUNNING) |
| ? kOperatingVoltage |
| : kZeroingVoltage; |
| if (unsafe_goal) { |
| constexpr float kTriggerThreshold = 12.0 * 0.90 / 0.005; |
| |
| if (unsafe_goal->voltage_climber() > 1.0) { |
| kill_shoulder_accumulator_ += |
| ::std::min(kMaxClimberVoltage, unsafe_goal->voltage_climber()); |
| } else { |
| kill_shoulder_accumulator_ = 0.0; |
| } |
| |
| if (kill_shoulder_accumulator_ > kTriggerThreshold) { |
| kill_shoulder_ = true; |
| } |
| } |
| arm_.set_max_voltage( |
| {{kill_shoulder_ ? 0.0 : max_voltage, |
| kill_shoulder_ ? (arm_.X_hat(0, 0) < 0.05 ? kShooterHangingLowVoltage |
| : kShooterHangingVoltage) |
| : max_voltage}}); |
| intake_.set_max_voltage({{max_voltage}}); |
| |
| if (IsRunning() && !kill_shoulder_) { |
| // We don't want lots of negative voltage when we are near the bellypan on |
| // the shoulder... |
| // TODO(austin): Do I want to push negative power into the belly pan at this |
| // point? Maybe just put the goal slightly below the bellypan and call that |
| // good enough. |
| if (arm_.goal(0, 0) <= kShoulderTransitionToLanded + 1e-4 || |
| arm_.X_hat(0, 0) <= kShoulderTransitionToLanded + 1e-4) { |
| arm_.set_shoulder_asymetric_limits(kLandingShoulderDownVoltage, |
| max_voltage); |
| } |
| } |
| |
| // Calculate the loops for a cycle. |
| double intake_position_power; |
| double intake_velocity_power; |
| { |
| Eigen::Matrix<double, 3, 1> error = intake_.controller().error(); |
| intake_position_power = |
| intake_.controller().controller().K(0, 0) * error(0, 0); |
| intake_velocity_power = |
| intake_.controller().controller().K(0, 1) * error(1, 0); |
| } |
| |
| double shoulder_position_power; |
| double shoulder_velocity_power; |
| double wrist_position_power; |
| double wrist_velocity_power; |
| { |
| Eigen::Matrix<double, 6, 1> error = arm_.controller().error(); |
| shoulder_position_power = |
| arm_.controller().controller().K(0, 0) * error(0, 0); |
| shoulder_velocity_power = |
| arm_.controller().controller().K(0, 1) * error(1, 0); |
| wrist_position_power = arm_.controller().controller().K(0, 2) * error(2, 0); |
| wrist_velocity_power = arm_.controller().controller().K(0, 3) * error(3, 0); |
| } |
| |
| arm_.Update(disable); |
| intake_.Update(disable); |
| |
| // Write out all the voltages. |
| if (output) { |
| OutputT output_struct; |
| output_struct.voltage_intake = intake_.voltage(); |
| output_struct.voltage_shoulder = arm_.shoulder_voltage(); |
| output_struct.voltage_wrist = arm_.wrist_voltage(); |
| |
| output_struct.voltage_top_rollers = 0.0; |
| output_struct.voltage_bottom_rollers = 0.0; |
| |
| output_struct.voltage_climber = 0.0; |
| output_struct.unfold_climber = false; |
| if (unsafe_goal) { |
| // Ball detector lights. |
| ball_detector_fetcher_.Fetch(); |
| bool ball_detected = false; |
| if (ball_detector_fetcher_.get()) { |
| ball_detected = ball_detector_fetcher_->voltage() > 2.5; |
| } |
| |
| // Climber. |
| output_struct.voltage_climber = ::std::max( |
| static_cast<float>(0.0), |
| ::std::min(unsafe_goal->voltage_climber(), kMaxClimberVoltage)); |
| output_struct.unfold_climber = unsafe_goal->unfold_climber(); |
| |
| // Intake. |
| if (unsafe_goal->force_intake() || !ball_detected) { |
| output_struct.voltage_top_rollers = ::std::max( |
| -kMaxIntakeTopVoltage, |
| ::std::min(unsafe_goal->voltage_top_rollers(), kMaxIntakeTopVoltage)); |
| output_struct.voltage_bottom_rollers = |
| ::std::max(-kMaxIntakeBottomVoltage, |
| ::std::min(unsafe_goal->voltage_bottom_rollers(), |
| kMaxIntakeBottomVoltage)); |
| } else { |
| output_struct.voltage_top_rollers = 0.0; |
| output_struct.voltage_bottom_rollers = 0.0; |
| } |
| |
| // Traverse. |
| output_struct.traverse_unlatched = unsafe_goal->traverse_unlatched(); |
| output_struct.traverse_down = unsafe_goal->traverse_down(); |
| } |
| |
| output->Send(Output::Pack(*output->fbb(), &output_struct)); |
| } |
| |
| // Save debug/internal state. |
| flatbuffers::Offset<frc971::EstimatorState> shoulder_estimator_state_offset = |
| arm_.EstimatorState(status->fbb(), 0); |
| |
| JointState::Builder shoulder_builder = status->MakeBuilder<JointState>(); |
| |
| shoulder_builder.add_angle(arm_.X_hat(0, 0)); |
| shoulder_builder.add_angular_velocity(arm_.X_hat(1, 0)); |
| shoulder_builder.add_goal_angle(arm_.goal(0, 0)); |
| shoulder_builder.add_goal_angular_velocity(arm_.goal(1, 0)); |
| shoulder_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(0, 0)); |
| shoulder_builder.add_unprofiled_goal_angular_velocity( |
| arm_.unprofiled_goal(1, 0)); |
| shoulder_builder.add_voltage_error(arm_.X_hat(4, 0)); |
| shoulder_builder.add_calculated_velocity( |
| (arm_.shoulder_angle() - last_shoulder_angle_) / 0.005); |
| shoulder_builder.add_position_power(shoulder_position_power); |
| shoulder_builder.add_velocity_power(shoulder_velocity_power); |
| shoulder_builder.add_estimator_state(shoulder_estimator_state_offset); |
| |
| flatbuffers::Offset<JointState> shoulder_offset = shoulder_builder.Finish(); |
| |
| flatbuffers::Offset<frc971::EstimatorState> wrist_estimator_state_offset = |
| arm_.EstimatorState(status->fbb(), 1); |
| |
| JointState::Builder wrist_builder = status->MakeBuilder<JointState>(); |
| |
| wrist_builder.add_angle(arm_.X_hat(2, 0)); |
| wrist_builder.add_angular_velocity(arm_.X_hat(3, 0)); |
| wrist_builder.add_goal_angle(arm_.goal(2, 0)); |
| wrist_builder.add_goal_angular_velocity(arm_.goal(3, 0)); |
| wrist_builder.add_unprofiled_goal_angle(arm_.unprofiled_goal(2, 0)); |
| wrist_builder.add_unprofiled_goal_angular_velocity( |
| arm_.unprofiled_goal(3, 0)); |
| wrist_builder.add_voltage_error(arm_.X_hat(5, 0)); |
| wrist_builder.add_calculated_velocity( |
| (arm_.wrist_angle() - last_wrist_angle_) / 0.005); |
| wrist_builder.add_position_power(wrist_position_power); |
| wrist_builder.add_velocity_power(wrist_velocity_power); |
| wrist_builder.add_estimator_state(wrist_estimator_state_offset); |
| |
| flatbuffers::Offset<JointState> wrist_offset = wrist_builder.Finish(); |
| |
| flatbuffers::Offset<frc971::EstimatorState> intake_estimator_state_offset = |
| intake_.EstimatorState(status->fbb(), 0); |
| |
| JointState::Builder intake_builder = status->MakeBuilder<JointState>(); |
| intake_builder.add_position_power(intake_position_power); |
| intake_builder.add_velocity_power(intake_velocity_power); |
| intake_builder.add_angle(intake_.X_hat(0, 0)); |
| intake_builder.add_angular_velocity(intake_.X_hat(1, 0)); |
| intake_builder.add_goal_angle(intake_.goal(0, 0)); |
| intake_builder.add_goal_angular_velocity(intake_.goal(1, 0)); |
| intake_builder.add_unprofiled_goal_angle(intake_.unprofiled_goal(0, 0)); |
| intake_builder.add_unprofiled_goal_angular_velocity( |
| intake_.unprofiled_goal(1, 0)); |
| intake_builder.add_calculated_velocity( |
| (intake_.position() - last_intake_angle_) / 0.005); |
| intake_builder.add_voltage_error(intake_.X_hat(2, 0)); |
| intake_builder.add_estimator_state(intake_estimator_state_offset); |
| intake_builder.add_feedforwards_power(intake_.controller().ff_U(0, 0)); |
| |
| flatbuffers::Offset<JointState> intake_offset = intake_builder.Finish(); |
| |
| Status::Builder status_builder = status->MakeBuilder<Status>(); |
| |
| status_builder.add_shoulder(shoulder_offset); |
| status_builder.add_wrist(wrist_offset); |
| status_builder.add_intake(intake_offset); |
| |
| status_builder.add_zeroed(arm_.zeroed() && intake_.zeroed()); |
| status_builder.add_shoulder_controller_index(arm_.controller_index()); |
| |
| last_shoulder_angle_ = arm_.shoulder_angle(); |
| last_wrist_angle_ = arm_.wrist_angle(); |
| last_intake_angle_ = intake_.position(); |
| |
| status_builder.add_estopped((state_ == ESTOP)); |
| |
| status_builder.add_state(state_); |
| status_builder.add_is_collided(is_collided); |
| |
| status->Send(status_builder.Finish()); |
| |
| last_state_ = state_before_switch; |
| } |
| |
| constexpr double Superstructure::kZeroingVoltage; |
| constexpr double Superstructure::kOperatingVoltage; |
| constexpr double Superstructure::kLandingShoulderDownVoltage; |
| constexpr double Superstructure::kShoulderMiddleAngle; |
| constexpr double Superstructure::kLooseTolerance; |
| constexpr double Superstructure::kIntakeUpperClear; |
| constexpr double Superstructure::kIntakeLowerClear; |
| constexpr double Superstructure::kShoulderUpAngle; |
| constexpr double Superstructure::kShoulderLanded; |
| constexpr double Superstructure::kTightTolerance; |
| constexpr double Superstructure::kWristAlmostLevel; |
| constexpr double Superstructure::kShoulderWristClearAngle; |
| constexpr double Superstructure::kShoulderTransitionToLanded; |
| |
| } // namespace superstructure |
| } // namespace control_loops |
| } // namespace y2016 |