frc971/control_loops/claw/claw.cc

#include "frc971/control_loops/claw/claw.h"

#include <stdio.h>

#include <algorithm>

#include "aos/common/control_loop/control_loops.q.h"
#include "aos/common/logging/logging.h"

#include "frc971/constants.h"
#include "frc971/control_loops/claw/top_claw_motor_plant.h"
#include "frc971/control_loops/claw/bottom_claw_motor_plant.h"

// Zeroing plan.
// There are 2 types of zeros.  Enabled and disabled ones.
// Disabled ones are only valid during auto mode, and can be used to speed up
// the enabled zero process.  We need to re-zero during teleop in case the auto
// zero was poor and causes us to miss all our shots.
//
// We need to be able to zero manually while disabled by moving the joint over
// the zeros.
// Zero on the down edge when disabled (gravity in the direction of motion)
//
// When enabled, zero on the up edge (gravity opposing the direction of motion)
// The enabled sequence needs to work as follows.  We can crash the claw if we
// bring them too close to each other or too far from each other.  The only safe
// thing to do is to move them in unison.
//
// Start by moving them both towards the front of the bot to either find either
// the middle hall effect on either jaw, or the front hall effect on the bottom
// jaw.  Any edge that isn't the desired edge will provide an approximate edge
// location that can be used for the fine tuning step.
// Once an edge is found on the front claw, move back the other way with both
// claws until an edge is found for the other claw.
// Now that we have an approximate zero, we can robustify the limits to keep
// both claws safe.  Then, we can move both claws to a position that is the
// correct side of the zero and go zero.

// Valid region plan.
// Difference between the arms has a range, and the values of each arm has a range.
// If a claw runs up against a static limit, don't let the goal change outside
// the limit.
// If a claw runs up against a movable limit, move both claws outwards to get
// out of the condition.

namespace frc971 {
namespace control_loops {

void ZeroedStateFeedbackLoop::CapU() {
  const double old_voltage = voltage_;
  voltage_ += U(0, 0);

  uncapped_voltage_ = voltage_;

  double limit = zeroing_state() != UNKNOWN_POSITION ? 12.0 : kZeroingMaxVoltage;

  // Make sure that reality and the observer can't get too far off.  There is a
  // delay by one cycle between the applied voltage and X_hat(2, 0), so compare
  // against last cycle's voltage.
  if (X_hat(2, 0) > last_voltage_ + 2.0) {
    voltage_ -= X_hat(2, 0) - (last_voltage_ + 2.0);
    LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
  } else if (X_hat(2, 0) < last_voltage_ -2.0) {
    voltage_ += X_hat(2, 0) - (last_voltage_ - 2.0);
    LOG(DEBUG, "X_hat(2, 0) = %f\n", X_hat(2, 0));
  }

  voltage_ = std::min(limit, voltage_);
  voltage_ = std::max(-limit, voltage_);
  U(0, 0) = voltage_ - old_voltage;
  LOG(DEBUG, "abc %f\n", X_hat(2, 0) - voltage_);
  LOG(DEBUG, "error %f\n", X_hat(0, 0) - R(0, 0));

  last_voltage_ = voltage_;
}

ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
    : aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
      has_top_claw_goal_(false),
      top_claw_goal_(0.0),
      top_claw_(MakeTopClawLoop()),
      has_bottom_claw_goal_(false),
      bottom_claw_goal_(0.0),
      bottom_claw_(MakeBottomClawLoop()),
      was_enabled_(false),
      doing_calibration_fine_tune_(false) {}

const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;

bool ZeroedStateFeedbackLoop::GetPositionOfEdge(
    const constants::Values::Claw &claw_values, double *edge_encoder,
    double *edge_angle) {

  // TODO(austin): Validate that the hall effect edge makes sense.
  // We must now be on the side of the edge that we expect to be, and the
  // encoder must have been on either side of the edge before and after.

  if (front_hall_effect_posedge_count_changed()) {
    if (encoder() - last_encoder() < 0) {
      *edge_angle = claw_values.front.upper_angle;
    } else {
      *edge_angle = claw_values.front.lower_angle;
    }
    *edge_encoder = posedge_value_;
    return true;
  }
  if (front_hall_effect_negedge_count_changed()) {
    if (encoder() - last_encoder() > 0) {
      *edge_angle = claw_values.front.upper_angle;
    } else {
      *edge_angle = claw_values.front.lower_angle;
    }
    *edge_encoder = negedge_value_;
    return true;
  }
  if (calibration_hall_effect_posedge_count_changed()) {
    if (encoder() - last_encoder() < 0) {
      *edge_angle = claw_values.calibration.upper_angle;
    } else {
      *edge_angle = claw_values.calibration.lower_angle;
    }
    *edge_encoder = posedge_value_;
    return true;
  }
  if (calibration_hall_effect_negedge_count_changed()) {
    if (encoder() - last_encoder() > 0) {
      *edge_angle = claw_values.calibration.upper_angle;
    } else {
      *edge_angle = claw_values.calibration.lower_angle;
    }
    *edge_encoder = negedge_value_;
    return true;
  }
  if (back_hall_effect_posedge_count_changed()) {
    if (encoder() - last_encoder() < 0) {
      *edge_angle = claw_values.back.upper_angle;
    } else {
      *edge_angle = claw_values.back.lower_angle;
    }
    *edge_encoder = posedge_value_;
    return true;
  }
  if (back_hall_effect_negedge_count_changed()) {
    if (encoder() - last_encoder() > 0) {
      *edge_angle = claw_values.back.upper_angle;
    } else {
      *edge_angle = claw_values.back.lower_angle;
    }
    *edge_encoder = negedge_value_;
    return true;
  }
  return false;
}

// Positive angle is up, and positive power is up.
void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
                             const control_loops::ClawGroup::Position *position,
                             control_loops::ClawGroup::Output *output,
                             ::aos::control_loops::Status *status) {
  constexpr double dt = 0.01;

  // Disable the motors now so that all early returns will return with the
  // motors disabled.
  if (output) {
    output->top_claw_voltage = 0;
    output->bottom_claw_voltage = 0;
    output->intake_voltage = 0;
  }

  // TODO(austin): Handle the disabled state and the disabled -> enabled
  // transition in all of these states.
  // TODO(austin): Handle zeroing while disabled.

  // TODO(austin): Save all the counters so we know when something actually
  //               happens.
  // TODO(austin): Helpers to find the position of the claw on an edge.

  // TODO(austin): This may not be necesary because of the ControlLoop class.
  ::aos::robot_state.FetchLatest();
  if (::aos::robot_state.get() == nullptr) {
    return;
  }

  const frc971::constants::Values &values = constants::GetValues();

  if (position) {
    top_claw_.SetPositionValues(position->top);
    bottom_claw_.SetPositionValues(position->bottom);

    if (!has_top_claw_goal_) {
      has_top_claw_goal_ = true;
      top_claw_goal_ = position->top.position;
    }
    if (!has_bottom_claw_goal_) {
      has_bottom_claw_goal_ = true;
      bottom_claw_goal_ = position->bottom.position;
    }
  }

  bool autonomous = ::aos::robot_state->autonomous;
  bool enabled = ::aos::robot_state->enabled;

  if ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
       bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
      (autonomous &&
       ((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
         top_claw_.zeroing_state() ==
             ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
        (bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
         bottom_claw_.zeroing_state() ==
             ZeroedStateFeedbackLoop::DISABLED_CALIBRATION)))) {
    // Ready to use the claw.
    // Limit the goals here.
    bottom_claw_goal_ = goal->bottom_angle;
    top_claw_goal_ = goal->bottom_angle + goal->seperation_angle;
  } else if (top_claw_.zeroing_state() !=
                 ZeroedStateFeedbackLoop::UNKNOWN_POSITION &&
             bottom_claw_.zeroing_state() !=
                 ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
    // Time to fine tune the zero.
    // Limit the goals here.
    if (bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::CALIBRATED) {
        // always get the bottom claw to calibrated first
		if (!doing_calibration_fine_tune_) {
			if (position->bottom.position > values.start_fine_tune_pos - 
					values.claw_unimportant_epsilon &&
					position->bottom.position < values.start_fine_tune_pos + 
					values.claw_unimportant_epsilon) {
				doing_calibration_fine_tune_ = true;
        		bottom_claw_goal_ += values.claw_zeroing_off_speed * dt;
			} else {
				// send bottom to zeroing start
        		bottom_claw_goal_ = values.start_fine_tune_pos;
			}
		} else {
			if (position->bottom.front_hall_effect ||
					position->bottom.back_hall_effect ||
					position->top.front_hall_effect ||
					position->top.back_hall_effect) {
				// this should not happen, but now we know it won't
				doing_calibration_fine_tune_ = false;
				bottom_claw_goal_ = values.start_fine_tune_pos;
			}
			if (position->bottom.calibration_hall_effect) {
                // do calibration
      			bottom_claw_.SetCalibration(
				    position->bottom.posedge_value,
         	 		    values.lower_claw.calibration.lower_angle);
				bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
                // calinrated so we are done fine tuning bottom
				doing_calibration_fine_tune_ = false;
			}
		}
        // now set the top claw to track
        top_claw_goal_ = bottom_claw_goal_ + goal->seperation_angle;
    } else {
        // bottom claw must be calibrated, start on the top
		if (!doing_calibration_fine_tune_) {
			if (position->top.position > values.start_fine_tune_pos - 
					values.claw_unimportant_epsilon &&
					position->top.position < values.start_fine_tune_pos + 
					values.claw_unimportant_epsilon) {
				doing_calibration_fine_tune_ = true;
        		top_claw_goal_ += values.claw_zeroing_off_speed * dt;
			} else {
				// send top to zeroing start
        		top_claw_goal_ = values.start_fine_tune_pos;
			}
		} else {
			if (position->top.front_hall_effect ||
					position->top.back_hall_effect ||
					position->top.front_hall_effect ||
					position->top.back_hall_effect) {
				// this should not happen, but now we know it won't
				doing_calibration_fine_tune_ = false;
				top_claw_goal_ = values.start_fine_tune_pos;
			}
			if (position->top.calibration_hall_effect) {
                // do calibration
      			top_claw_.SetCalibration(
				    position->top.posedge_value,
         	 		    values.upper_claw.calibration.lower_angle);
				top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
                // calinrated so we are done fine tuning top
				doing_calibration_fine_tune_ = false;
			}
		}
        // now set the bottom claw to track
        bottom_claw_goal_ = top_claw_goal_ - goal->seperation_angle;
    }
  } else {
    if (!was_enabled_ && enabled) {
      if (position) {
        top_claw_goal_ = position->top.position;
        bottom_claw_goal_ = position->bottom.position;
      } else {
        has_top_claw_goal_ = false;
        has_bottom_claw_goal_ = false;
      }
    }

    // TODO(austin): Limit the goals here.
    // Need to prevent windup, limit voltage, deal with windup on only 1 claw,
    // ...
    if (top_claw_.zeroing_state() ==
        ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
    }
    if (bottom_claw_.zeroing_state() ==
        ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
    }

    if (bottom_claw_.zeroing_state() !=
        ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
      if (enabled) {
        // Time to slowly move back up to find any position to narrow down the
        // zero.
        top_claw_goal_ += values.claw_zeroing_off_speed * dt;
        bottom_claw_goal_ += values.claw_zeroing_off_speed * dt;
        // TODO(austin): Goal velocity too!
      }
    } else {
      // We don't know where either claw is.  Slowly start moving down to find
      // any hall effect.
      if (enabled) {
        top_claw_goal_-= values.claw_zeroing_off_speed * dt;
        bottom_claw_goal_ -= values.claw_zeroing_off_speed * dt;
        // TODO(austin): Goal velocity too!
      }
    }

    if (enabled) {
      top_claw_.SetCalibrationOnEdge(
          values.upper_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
      bottom_claw_.SetCalibrationOnEdge(
          values.lower_claw, ZeroedStateFeedbackLoop::APPROXIMATE_CALIBRATION);
    } else {
      top_claw_.SetCalibrationOnEdge(
          values.upper_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
      bottom_claw_.SetCalibrationOnEdge(
          values.lower_claw, ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
    }
  }

  // TODO(austin): Handle disabled.

  // TODO(austin): ...
  if (has_top_claw_goal_ && has_bottom_claw_goal_) {
    top_claw_.R << top_claw_goal_, 0.0, 0.0;
    bottom_claw_.R << bottom_claw_goal_, 0.0, 0.0;

    top_claw_.Update(output == nullptr);
    bottom_claw_.Update(output == nullptr);
  } else {
    top_claw_.ZeroPower();
    bottom_claw_.ZeroPower();
  }

  if (position) {
    //LOG(DEBUG, "pos: %f hall: %s absolute: %f\n", position->top_position,
        //position->top_calibration_hall_effect ? "true" : "false",
        //zeroed_joint_.absolute_position());
  }

  if (output) {
    output->top_claw_voltage = top_claw_.voltage();
    output->bottom_claw_voltage = bottom_claw_.voltage();
  }
  status->done = false;
      //::std::abs(zeroed_joint_.absolute_position() - goal->bottom_angle -
                 //goal->seperation_angle) < 0.004;

  was_enabled_ = ::aos::robot_state->enabled;
}

}  // namespace control_loops
}  // namespace frc971