got 2 balls in 2 goal auto working
diff --git a/aos/common/commonmath.h b/aos/common/commonmath.h
index a77210f..f9337b2 100644
--- a/aos/common/commonmath.h
+++ b/aos/common/commonmath.h
@@ -4,7 +4,7 @@
namespace aos {
// Clips a value so that it is in [min, max]
-inline double Clip(double value, double min, double max) {
+static inline double Clip(double value, double min, double max) {
if (value > max) {
value = max;
} else if (value < min) {
@@ -13,6 +13,15 @@
return value;
}
+template <typename T>
+static inline int sign(T val) {
+ if (val > T(0)) {
+ return 1;
+ } else {
+ return -1;
+ }
+}
+
} // namespace aos
#endif // AOS_COMMON_MATH_H_
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
index 8a4557a..24a766c 100644
--- a/frc971/actions/drivetrain_action.cc
+++ b/frc971/actions/drivetrain_action.cc
@@ -19,37 +19,52 @@
void DrivetrainAction::RunAction() {
const double yoffset = action_q_->goal->y_offset;
- LOG(INFO, "Going to move %f\n", yoffset);
+ const double turn_offset =
+ action_q_->goal->theta_offset * constants::GetValues().turn_width / 2.0;
+ LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
- ::Eigen::Matrix<double, 2, 1> profile_goal_state;
+ ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(10));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
+ ::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
- profile.set_maximum_acceleration(2.2);
+ profile.set_maximum_acceleration(3.0);
profile.set_maximum_velocity(action_q_->goal->maximum_velocity);
+ turn_profile.set_maximum_acceleration(
+ 10.0 * constants::GetValues().turn_width / 2.0);
+ turn_profile.set_maximum_velocity(3.0 * constants::GetValues().turn_width /
+ 2.0);
while (true) {
// wait until next 10ms tick
::aos::time::PhasedLoop10MS(5000);
- profile_goal_state = profile.Update(yoffset, goal_velocity);
+ const auto drive_profile_goal_state =
+ profile.Update(yoffset, goal_velocity);
+ const auto turn_profile_goal_state = turn_profile.Update(turn_offset, 0.0);
+ left_goal_state = drive_profile_goal_state - turn_profile_goal_state;
+ right_goal_state = drive_profile_goal_state + turn_profile_goal_state;
- if (::std::abs(profile_goal_state(0, 0) - yoffset) < epsilon) break;
+ if (::std::abs(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
+ ::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
+ break;
+ }
if (ShouldCancel()) return;
LOG(DEBUG, "Driving left to %f, right to %f\n",
- profile_goal_state(0, 0) + action_q_->goal->left_initial_position,
- profile_goal_state(0, 0) + action_q_->goal->right_initial_position);
+ left_goal_state(0, 0) + action_q_->goal->left_initial_position,
+ right_goal_state(0, 0) + action_q_->goal->right_initial_position);
control_loops::drivetrain.goal.MakeWithBuilder()
.control_loop_driving(true)
.highgear(false)
- .left_goal(profile_goal_state(0, 0) + action_q_->goal->left_initial_position)
- .right_goal(profile_goal_state(0, 0) + action_q_->goal->right_initial_position)
- .left_velocity_goal(profile_goal_state(1, 0))
- .right_velocity_goal(profile_goal_state(1, 0))
+ .left_goal(left_goal_state(0, 0) + action_q_->goal->left_initial_position)
+ .right_goal(right_goal_state(0, 0) + action_q_->goal->right_initial_position)
+ .left_velocity_goal(left_goal_state(1, 0))
+ .right_velocity_goal(right_goal_state(1, 0))
.Send();
}
+ if (ShouldCancel()) return;
control_loops::drivetrain.status.FetchLatest();
while (!control_loops::drivetrain.status.get()) {
LOG(WARNING,
@@ -63,10 +78,10 @@
const double left_error = ::std::abs(
control_loops::drivetrain.status->filtered_left_position -
- (profile_goal_state(0, 0) + action_q_->goal->left_initial_position));
+ (left_goal_state(0, 0) + action_q_->goal->left_initial_position));
const double right_error = ::std::abs(
control_loops::drivetrain.status->filtered_right_position -
- (profile_goal_state(0, 0) + action_q_->goal->right_initial_position));
+ (right_goal_state(0, 0) + action_q_->goal->right_initial_position));
const double velocity_error =
::std::abs(control_loops::drivetrain.status->robot_speed);
if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
diff --git a/frc971/actions/drivetrain_action.q b/frc971/actions/drivetrain_action.q
index f61996f..3e89919 100644
--- a/frc971/actions/drivetrain_action.q
+++ b/frc971/actions/drivetrain_action.q
@@ -10,6 +10,7 @@
double left_initial_position;
double right_initial_position;
double y_offset;
+ double theta_offset;
double maximum_velocity;
};
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index e6cc9a4..82f690d 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -160,17 +160,20 @@
}
::std::unique_ptr<TypedAction< ::frc971::actions::DrivetrainActionQueueGroup>>
-SetDriveGoal(double distance, double maximum_velocity = 1.7) {
+SetDriveGoal(double distance, double maximum_velocity = 1.7, double theta = 0) {
LOG(INFO, "Driving to %f\n", distance);
auto drivetrain_action = actions::MakeDrivetrainAction();
drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
drivetrain_action->GetGoal()->y_offset = distance;
+ drivetrain_action->GetGoal()->theta_offset = theta;
drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
drivetrain_action->Start();
- // Uncomment to make relative again.
- left_initial_position += distance;
- right_initial_position += distance;
+ left_initial_position +=
+ distance - theta * constants::GetValues().turn_width / 2.0;
+ right_initial_position +=
+ distance + theta * constants::GetValues().turn_width / 2. -
+ theta * constants::GetValues().turn_width / 2.00;
return ::std::move(drivetrain_action);
}
@@ -219,8 +222,9 @@
void HandleAuto() {
// The front of the robot is 1.854 meters from the wall
- const double kShootDistance = 3.15;
- const double kPickupDistance = 0.5;
+ static const double kShootDistance = 3.15;
+ static const double kPickupDistance = 0.5;
+ static const double kTurnAngle = 0.3;
::aos::time::Time start_time = ::aos::time::Time::Now();
LOG(INFO, "Handling auto mode\n");
ResetDrivetrain();
@@ -250,15 +254,29 @@
WaitUntilDoneOrCanceled(drivetrain_action.get());
if (ShouldExitAuto()) return;
}
- LOG(INFO, "Shooting at %f\n",
- (::aos::time::Time::Now() - start_time).ToSeconds());
+
+ {
+ if (ShouldExitAuto()) return;
+ auto drivetrain_action = SetDriveGoal(0, 0, kTurnAngle);
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
// Shoot.
+ LOG(INFO, "Shooting at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
Shoot();
time::SleepFor(time::Time::InSeconds(0.05));
{
if (ShouldExitAuto()) return;
+ auto drivetrain_action = SetDriveGoal(0, 0, -kTurnAngle);
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+
+ {
+ if (ShouldExitAuto()) return;
// Intake the new ball.
LOG(INFO, "Claw ready for intake at %f\n",
(::aos::time::Time::Now() - start_time).ToSeconds());
@@ -287,6 +305,13 @@
if (ShouldExitAuto()) return;
}
+ {
+ if (ShouldExitAuto()) return;
+ auto drivetrain_action = SetDriveGoal(0, 0, -kTurnAngle);
+ WaitUntilDoneOrCanceled(drivetrain_action.get());
+ if (ShouldExitAuto()) return;
+ }
+
LOG(INFO, "Shooting at %f\n",
(::aos::time::Time::Now() - start_time).ToSeconds());
// Shoot
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 6057756..829d147 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -37,6 +37,8 @@
const ShifterHallEffect kPracticeRightDriveShifter{550, 640, 635, 550, 0.2, 0.7};
const ShifterHallEffect kPracticeLeftDriveShifter{540, 620, 640, 550, 0.2, 0.7};
+const double kRobotWidth = 25.0 / 100.0 * 2.54;
+
const double shooter_zeroing_speed = 0.05;
const double shooter_unload_speed = 0.08;
@@ -53,6 +55,7 @@
kCompLeftDriveShifter,
kCompRightDriveShifter,
false,
+ 0.5,
control_loops::MakeVelocityDrivetrainLoop,
control_loops::MakeDrivetrainLoop,
// ShooterLimits
@@ -88,6 +91,7 @@
kCompLeftDriveShifter,
kCompRightDriveShifter,
false,
+ kRobotWidth,
control_loops::MakeVelocityDrivetrainLoop,
control_loops::MakeDrivetrainLoop,
// ShooterLimits
@@ -132,6 +136,7 @@
kPracticeLeftDriveShifter,
kPracticeRightDriveShifter,
false,
+ kRobotWidth,
control_loops::MakeVelocityDrivetrainLoop,
control_loops::MakeDrivetrainLoop,
// ShooterLimits
diff --git a/frc971/constants.h b/frc971/constants.h
index 3a37aa7..67c6a16 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -44,6 +44,8 @@
bool clutch_transmission;
+ double turn_width;
+
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 6538a26..84939a9 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -6,6 +6,7 @@
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
#include "aos/common/logging/matrix_logging.h"
+#include "aos/common/commonmath.h"
#include "frc971/constants.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
@@ -45,17 +46,6 @@
namespace frc971 {
namespace control_loops {
-namespace {
-
-template <typename T> int sign(T val) {
- if (val > T(0)) {
- return 1;
- } else {
- return -1;
- }
-}
-
-} // namespace
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
@@ -89,7 +79,7 @@
// constrains the region such that, if at all possible, it will maintain its
// current efforts to reduce velocity error.
void ClawLimitedLoop::CapU() {
- Eigen::Matrix<double, 4, 1> error = R - X_hat;
+ const Eigen::Matrix<double, 4, 1> error = R - X_hat;
double u_top = U(1, 0);
double u_bottom = U(0, 0);
@@ -98,7 +88,7 @@
double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
- if (::std::abs(u_bottom) > max_voltage or ::std::abs(u_top) > max_voltage) {
+ if (::std::abs(u_bottom) > max_voltage || ::std::abs(u_top) > max_voltage) {
LOG_MATRIX(DEBUG, "U at start", U);
// H * U <= k
// U = UPos + UVel
@@ -148,7 +138,7 @@
angle_45 << ::std::sqrt(3), 1;
}
Eigen::Matrix<double, 1, 2> L45_quadrant;
- L45_quadrant << sign(P(1, 0)), -sign(P(0, 0));
+ L45_quadrant << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
const auto L45 = L45_quadrant.cwiseProduct(angle_45);
const double w45 = 0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 168f87f..9a84a08 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -24,13 +24,104 @@
namespace frc971 {
namespace control_loops {
-// Width of the robot.
-const double width = 25.0 / 100.0 * 2.54;
-
class DrivetrainMotorsSS {
public:
+ class LimitedDrivetrainLoop : public StateFeedbackLoop<4, 2, 2> {
+ public:
+ LimitedDrivetrainLoop(const StateFeedbackLoop<4, 2, 2> &loop)
+ : StateFeedbackLoop<4, 2, 2>(loop),
+ U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
+ -1, 0,
+ 0, 1,
+ 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0,
+ 12.0, 12.0).finished()) {
+ ::aos::controls::HPolytope<0>::Init();
+ T << 1, -1, 1, 1;
+ T_inverse = T.inverse();
+ }
+
+ private:
+ virtual void CapU() {
+ const Eigen::Matrix<double, 4, 1> error = R - X_hat;
+
+ if (U(0, 0) > 12.0 || U(1, 0) > 12.0) {
+ LOG_MATRIX(DEBUG, "U at start", U);
+
+ Eigen::Matrix<double, 2, 2> position_K;
+ position_K << K(0, 0), K(0, 2),
+ K(1, 0), K(1, 2);
+ Eigen::Matrix<double, 2, 2> velocity_K;
+ velocity_K << K(0, 1), K(0, 3),
+ K(1, 1), K(1, 3);
+
+ Eigen::Matrix<double, 2, 1> position_error;
+ position_error << error(0, 0), error(2, 0);
+ const auto drive_error = T_inverse * position_error;
+ Eigen::Matrix<double, 2, 1> velocity_error;
+ velocity_error << error(1, 0), error(3, 0);
+ LOG_MATRIX(DEBUG, "error", error);
+
+ const auto &poly = U_Poly_;
+ const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K * T;
+ const Eigen::Matrix<double, 4, 1> pos_poly_k =
+ poly.k() - poly.H() * velocity_K * velocity_error;
+ const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+
+ Eigen::Matrix<double, 2, 1> adjusted_pos_error;
+ {
+ const auto &P = drive_error;
+
+ Eigen::Matrix<double, 1, 2> L45;
+ L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
+ const double w45 = 0;
+
+ Eigen::Matrix<double, 1, 2> LH;
+ if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
+ LH << 0, 1;
+ } else {
+ LH << 1, 0;
+ }
+ const double wh = LH.dot(P);
+
+ Eigen::Matrix<double, 2, 2> standard;
+ standard << L45, LH;
+ Eigen::Matrix<double, 2, 1> W;
+ W << w45, wh;
+ const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
+
+ bool is_inside_h;
+ const auto adjusted_pos_error_h =
+ DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+ const auto adjusted_pos_error_45 =
+ DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+ if (pos_poly.IsInside(intersection)) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ if (is_inside_h) {
+ if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm()) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
+ }
+ }
+ }
+
+ LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
+ U = velocity_K * velocity_error + position_K * T * adjusted_pos_error;
+ LOG_MATRIX(DEBUG, "U is now", U);
+ }
+ }
+
+ const ::aos::controls::HPolytope<2> U_Poly_;
+ Eigen::Matrix<double, 2, 2> T, T_inverse;
+ };
+
DrivetrainMotorsSS()
- : loop_(new StateFeedbackLoop<4, 2, 2>(
+ : loop_(new LimitedDrivetrainLoop(
constants::GetValues().make_drivetrain_loop())),
filtered_offset_(0.0),
gyro_(0.0),
@@ -59,7 +150,8 @@
void SetPosition(
double left, double right, double gyro, bool control_loop_driving) {
// Decay the offset quickly because this gyro is great.
- const double offset = (right - left - gyro * width) / 2.0;
+ const double offset =
+ (right - left - gyro * constants::GetValues().turn_width) / 2.0;
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
control_loop_driving_ = control_loop_driving;
@@ -90,7 +182,6 @@
}
double GetEstimatedLeftEncoder() {
- // lets just call the average of left and right velocities close enough
return loop_->X_hat(0, 0);
}
@@ -108,7 +199,7 @@
}
private:
- ::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> loop_;
+ ::std::unique_ptr<LimitedDrivetrainLoop> loop_;
double filtered_offset_;
double gyro_;