Got angle adjust working with limiting the zeroing goal.
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 5b9ff68..865195e 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -9,10 +9,8 @@
'../control_loops/wrist/wrist.gyp:wrist',
'../control_loops/wrist/wrist.gyp:wrist_lib_test',
'../control_loops/control_loops.gyp:hall_effect_lib_test',
- '../control_loops/control_loops.gyp:angle_adjust_lib_test',
- '../control_loops/control_loops.gyp:angle_adjust',
- '../control_loops/control_loops.gyp:wrist',
- '../control_loops/control_loops.gyp:wrist_lib_test',
+ '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust',
+ '../control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_lib_test',
'../input/input.gyp:JoystickReader',
'../input/input.gyp:SensorReader',
'../input/input.gyp:GyroReader',
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 6c7c54b..1b53c06 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -32,23 +32,6 @@
const double kPracticeWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
const double kCompWristLowerPhysicalLimit = -37.5 * M_PI / 180.0;
-const int kAngleAdjustHallEffect = 2;
-
-const ::std::array<double, kAngleAdjustHallEffect>
- kCompAngleAdjustHorizontalHallEffectStartAngle = {{-0.1, 1.0}};
-const ::std::array<double, kAngleAdjustHallEffect>
- kPracticeAngleAdjustHorizontalHallEffectStartAngle = {{-0.1, 1.0}};
-
-const ::std::array<double, kAngleAdjustHallEffect>
- kCompAngleAdjustHorizontalHallEffectStopAngle = {{0.5, 1.5}};
-const ::std::array<double, kAngleAdjustHallEffect>
- kPracticeAngleAdjustHorizontalHallEffectStopAngle = {{0.5, 1.5}};
-
-const double kPracticeAngleAdjustHorizontalUpperPhysicalLimit =
- 3.0;
-const double kCompAngleAdjustHorizontalUpperPhysicalLimit =
- 3.0;
-
const double kPracticeWristUpperLimit = 93 * M_PI / 180.0;
const double kCompWristUpperLimit = 93 * M_PI / 180.0;
@@ -57,22 +40,31 @@
const double kWristZeroingSpeed = 1.0;
-const double kPracticeAngleAdjustHorizontalLowerPhysicalLimit =
- 0.0;
-const double kCompAngleAdjustHorizontalLowerPhysicalLimit =
- 0.0;
+const int kAngleAdjustHallEffect = 2;
-const double kPracticeAngleAdjustHorizontalUpperLimit =
- 3.0;
-const double kCompAngleAdjustHorizontalUpperLimit =
- 3.0;
+const ::std::array<double, kAngleAdjustHallEffect>
+ kCompAngleAdjustHallEffectStartAngle = {{-0.1, 1.0}};
+const ::std::array<double, kAngleAdjustHallEffect>
+ kPracticeAngleAdjustHallEffectStartAngle = {{-0.1, 1.0}};
-const double kPracticeAngleAdjustHorizontalLowerLimit =
- 0.0;
-const double kCompAngleAdjustHorizontalLowerLimit =
- 0.0;
+const ::std::array<double, kAngleAdjustHallEffect>
+ kCompAngleAdjustHallEffectStopAngle = {{0.5, 1.5}};
+const ::std::array<double, kAngleAdjustHallEffect>
+ kPracticeAngleAdjustHallEffectStopAngle = {{0.5, 1.5}};
-const double kAngleAdjustHorizontalZeroingSpeed = 1.0;
+const double kPracticeAngleAdjustUpperPhysicalLimit = 3.0;
+const double kCompAngleAdjustUpperPhysicalLimit = 3.0;
+
+const double kPracticeAngleAdjustLowerPhysicalLimit = 0.0;
+const double kCompAngleAdjustLowerPhysicalLimit = 0.0;
+
+const double kPracticeAngleAdjustUpperLimit = 3.0;
+const double kCompAngleAdjustUpperLimit = 3.0;
+
+const double kPracticeAngleAdjustLowerLimit = 0.0;
+const double kCompAngleAdjustLowerLimit = 0.0;
+
+const double kAngleAdjustZeroingSpeed = 1.0;
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
@@ -94,18 +86,18 @@
double wrist_zeroing_speed;
// AngleAdjust hall effect positive and negative edges.
- ::std::array<double, 2> angle_adjust_horizontal_hall_effect_start_angle;
- ::std::array<double, 2> angle_adjust_horizontal_hall_effect_stop_angle;
+ ::std::array<double, 2> angle_adjust_hall_effect_start_angle;
+ ::std::array<double, 2> angle_adjust_hall_effect_stop_angle;
// Upper and lower extreme limits of travel for the angle adjust.
- double angle_adjust_horizontal_upper_limit;
- double angle_adjust_horizontal_lower_limit;
+ double angle_adjust_upper_limit;
+ double angle_adjust_lower_limit;
// Physical limits. These are here for testing.
- double angle_adjust_horizontal_upper_physical_limit;
- double angle_adjust_horizontal_lower_physical_limit;
+ double angle_adjust_upper_physical_limit;
+ double angle_adjust_lower_physical_limit;
// Zeroing speed.
- double angle_adjust_horizontal_zeroing_speed;
+ double angle_adjust_zeroing_speed;
// what camera_center returns
int camera_center;
@@ -129,13 +121,13 @@
kCompWristUpperPhysicalLimit,
kCompWristLowerPhysicalLimit,
kWristZeroingSpeed,
- kCompAngleAdjustHorizontalHallEffectStartAngle,
- kCompAngleAdjustHorizontalHallEffectStopAngle,
- kCompAngleAdjustHorizontalUpperLimit,
- kCompAngleAdjustHorizontalLowerLimit,
- kCompAngleAdjustHorizontalUpperPhysicalLimit,
- kCompAngleAdjustHorizontalLowerPhysicalLimit,
- kAngleAdjustHorizontalZeroingSpeed,
+ kCompAngleAdjustHallEffectStartAngle,
+ kCompAngleAdjustHallEffectStopAngle,
+ kCompAngleAdjustUpperLimit,
+ kCompAngleAdjustLowerLimit,
+ kCompAngleAdjustUpperPhysicalLimit,
+ kCompAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
kCompCameraCenter};
break;
case kPracticeTeamNumber:
@@ -146,13 +138,13 @@
kPracticeWristUpperPhysicalLimit,
kPracticeWristLowerPhysicalLimit,
kWristZeroingSpeed,
- kPracticeAngleAdjustHorizontalHallEffectStartAngle,
- kPracticeAngleAdjustHorizontalHallEffectStopAngle,
- kPracticeAngleAdjustHorizontalUpperLimit,
- kPracticeAngleAdjustHorizontalLowerLimit,
- kPracticeAngleAdjustHorizontalUpperPhysicalLimit,
- kPracticeAngleAdjustHorizontalLowerPhysicalLimit,
- kAngleAdjustHorizontalZeroingSpeed,
+ kPracticeAngleAdjustHallEffectStartAngle,
+ kPracticeAngleAdjustHallEffectStopAngle,
+ kPracticeAngleAdjustUpperLimit,
+ kPracticeAngleAdjustLowerLimit,
+ kPracticeAngleAdjustUpperPhysicalLimit,
+ kPracticeAngleAdjustLowerPhysicalLimit,
+ kAngleAdjustZeroingSpeed,
kPracticeCameraCenter};
break;
default:
@@ -213,52 +205,52 @@
return true;
}
-bool angle_adjust_horizontal_hall_effect_start_angle(
+bool angle_adjust_hall_effect_start_angle(
::std::array<double, 2> *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->angle_adjust_horizontal_hall_effect_start_angle;
+ *angle = values->angle_adjust_hall_effect_start_angle;
return true;
}
-bool angle_adjust_horizontal_hall_effect_stop_angle(
+bool angle_adjust_hall_effect_stop_angle(
::std::array<double, 2> *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->angle_adjust_horizontal_hall_effect_stop_angle;
+ *angle = values->angle_adjust_hall_effect_stop_angle;
return true;
}
-bool angle_adjust_horizontal_upper_limit(double *angle) {
+bool angle_adjust_upper_limit(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->angle_adjust_horizontal_upper_limit;
+ *angle = values->angle_adjust_upper_limit;
return true;
}
-bool angle_adjust_horizontal_lower_limit(double *angle) {
+bool angle_adjust_lower_limit(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->angle_adjust_horizontal_lower_limit;
+ *angle = values->angle_adjust_lower_limit;
return true;
}
-bool angle_adjust_horizontal_upper_physical_limit(double *angle) {
+bool angle_adjust_upper_physical_limit(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->angle_adjust_horizontal_upper_physical_limit;
+ *angle = values->angle_adjust_upper_physical_limit;
return true;
}
-bool angle_adjust_horizontal_lower_physical_limit(double *angle) {
+bool angle_adjust_lower_physical_limit(double *angle) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *angle = values->angle_adjust_horizontal_lower_physical_limit;
+ *angle = values->angle_adjust_lower_physical_limit;
return true;
}
-bool angle_adjust_horizontal_zeroing_speed(double *speed) {
+bool angle_adjust_zeroing_speed(double *speed) {
const Values *const values = GetValues();
if (values == NULL) return false;
- *speed = values->angle_adjust_horizontal_zeroing_speed;
+ *speed = values->angle_adjust_zeroing_speed;
return true;
}
diff --git a/frc971/constants.h b/frc971/constants.h
index be166ac..1f3eb35 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -27,19 +27,19 @@
// Returns the speed to move the wrist at when zeroing in rad/sec
bool wrist_zeroing_speed(double *speed);
-bool angle_adjust_horizontal_hall_effect_start_angle(
+bool angle_adjust_hall_effect_start_angle(
::std::array<double, 2> *angle);
-bool angle_adjust_horizontal_hall_effect_stop_angle(
+bool angle_adjust_hall_effect_stop_angle(
::std::array<double, 2> *angle);
// These are the soft stops for up and down.
-bool angle_adjust_horizontal_lower_limit(double *angle);
-bool angle_adjust_horizontal_upper_limit(double *angle);
+bool angle_adjust_lower_limit(double *angle);
+bool angle_adjust_upper_limit(double *angle);
// These are the hard stops. Don't use these for anything but testing.
-bool angle_adjust_horizontal_lower_physical_limit(double *angle);
-bool angle_adjust_horizontal_upper_physical_limit(double *angle);
+bool angle_adjust_lower_physical_limit(double *angle);
+bool angle_adjust_upper_physical_limit(double *angle);
// Returns speed to move the angle adjust when zeroing, in rad/sec
-bool angle_adjust_horizontal_zeroing_speed(double *speed);
+bool angle_adjust_zeroing_speed(double *speed);
// Sets *center to how many pixels off center the vertical line
// on the camera view is.
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.cc b/frc971/control_loops/angle_adjust/angle_adjust.cc
index 8291595..6f11d89 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust.cc
@@ -1,6 +1,4 @@
#include "frc971/control_loops/angle_adjust/angle_adjust.h"
-#include "frc971/control_loops/hall_effect_loop.h"
-#include "frc971/control_loops/hall_effect_loop-inl.h"
#include <algorithm>
@@ -11,6 +9,8 @@
#include "aos/common/logging/logging.h"
#include "frc971/constants.h"
+#include "frc971/control_loops/hall_effect_loop.h"
+#include "frc971/control_loops/hall_effect_loop-inl.h"
#include "frc971/control_loops/angle_adjust/angle_adjust_motor_plant.h"
namespace frc971 {
@@ -20,7 +20,8 @@
control_loops::AngleAdjustLoop *my_angle_adjust)
: aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop>(
my_angle_adjust),
- hall_effect_(new StateFeedbackLoop<2, 1, 1>(MakeAngleAdjustLoop()), true),
+ hall_effect_(new StateFeedbackLoop<2, 1, 1>(MakeAngleAdjustLoop()),
+ true, 5.0),
error_count_(0),
time_(0.0) {
if (testing) {
@@ -28,25 +29,27 @@
}
}
+/*static*/ const double AngleAdjustMotor::dt = 0.01;
+
bool AngleAdjustMotor::FetchConstants() {
- if (!constants::angle_adjust_horizontal_lower_limit(
- &horizontal_lower_limit_)) {
- LOG(ERROR, "Failed to fetch the horizontal lower limit constant.\n");
+ if (!constants::angle_adjust_lower_limit(
+ &lower_limit_)) {
+ LOG(ERROR, "Failed to fetch the angle adjust lower limit constant.\n");
return false;
}
- if (!constants::angle_adjust_horizontal_upper_limit(
- &horizontal_upper_limit_)) {
- LOG(ERROR, "Failed to fetch the horizontal upper limit constant.\n");
+ if (!constants::angle_adjust_upper_limit(
+ &upper_limit_)) {
+ LOG(ERROR, "Failed to fetch the angle adjust upper limit constant.\n");
return false;
}
- if (!constants::angle_adjust_horizontal_hall_effect_stop_angle(
- &horizontal_hall_effect_stop_angle_)) {
+ if (!constants::angle_adjust_hall_effect_stop_angle(
+ &hall_effect_stop_angle_)) {
LOG(ERROR, "Failed to fetch the hall effect stop angle constants.\n");
return false;
}
- if (!constants::angle_adjust_horizontal_zeroing_speed(
- &horizontal_zeroing_speed_)) {
- LOG(ERROR, "Failed to fetch the horizontal zeroing speed constant.\n");
+ if (!constants::angle_adjust_zeroing_speed(
+ &zeroing_speed_)) {
+ LOG(ERROR, "Failed to fetch the angle adjust zeroing speed constant.\n");
return false;
}
@@ -54,24 +57,24 @@
}
double AngleAdjustMotor::ClipGoal(double goal) const {
- return std::min(horizontal_upper_limit_,
- std::max(horizontal_lower_limit_, goal));
+ return std::min(upper_limit_,
+ std::max(lower_limit_, goal));
}
double AngleAdjustMotor::LimitVoltage(double absolute_position,
double voltage) const {
if (hall_effect_.state_ == HallEffectLoop<2>::READY) {
- if (absolute_position >= horizontal_upper_limit_) {
+ if (absolute_position >= upper_limit_) {
voltage = std::min(0.0, voltage);
}
- if (absolute_position <= horizontal_lower_limit_) {
+ if (absolute_position <= lower_limit_) {
voltage = std::max(0.0, voltage);
}
}
double limit = (hall_effect_.state_ == HallEffectLoop<2>::READY) ? 12.0 : 5.0;
// TODO(aschuh): Remove this line when we are done testing.
- //limit = std::min(0.3, limit);
+ // limit = std::min(0.3, limit);
voltage = std::min(limit, voltage);
voltage = std::max(-limit, voltage);
return voltage;
@@ -115,16 +118,18 @@
hall_effect_sensors_[1] = position->middle_hall_effect;
calibration_values_[0] = position->bottom_calibration;
calibration_values_[1] = position->middle_calibration;
- absolute_position = position->before_angle;
+ absolute_position = position->bottom_angle;
}
- hall_effect_.UpdateZeros(horizontal_hall_effect_stop_angle_,
+ // Deals with all the zeroing stuff.
+ hall_effect_.UpdateZeros(hall_effect_stop_angle_,
hall_effect_sensors_,
calibration_values_,
- horizontal_zeroing_speed_,
+ zeroing_speed_,
absolute_position,
position != NULL);
+ // Only try to go to our goal if we are actually zeroed.
if (hall_effect_.state_ == HallEffectLoop<2>::READY) {
const double limited_goal = ClipGoal(goal->goal);
hall_effect_.loop_->R << limited_goal, 0.0;
@@ -133,9 +138,13 @@
// Update the observer.
hall_effect_.loop_->Update(position != NULL, output == NULL);
+ // Prevent the zeroing goal from running off. Needs to happen after
+ // U is calculated, hence why this is after the loop_->Update.
+ hall_effect_.LimitZeroingGoal();
+
if (position) {
LOG(DEBUG, "pos=%f bottom_hall: %s middle_hall: %s\n",
- position->before_angle,
+ position->bottom_angle,
position->bottom_hall_effect ? "true" : "false",
position->middle_hall_effect ? "true" : "false");
}
@@ -154,7 +163,7 @@
hall_effect_.loop_->RecordDatum("angle_adjust.csv", time_);
}
time_ += dt;
-} // RunIteration
+} // RunIteration
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust.h b/frc971/control_loops/angle_adjust/angle_adjust.h
index a1ae134..6fe55ff 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust.h
+++ b/frc971/control_loops/angle_adjust/angle_adjust.h
@@ -1,8 +1,8 @@
-#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_H_
-#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_H_
+#ifndef FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
+#define FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
-#include <memory>
#include <array>
+#include <memory>
#include "aos/common/control_loop/ControlLoop.h"
#include "frc971/control_loops/state_feedback_loop.h"
@@ -23,7 +23,7 @@
: public aos::control_loops::ControlLoop<control_loops::AngleAdjustLoop> {
public:
explicit AngleAdjustMotor(
- control_loops::AngleAdjustLoop *my_angle_adjust =
+ control_loops::AngleAdjustLoop *my_angle_adjust =
&control_loops::angle_adjust);
protected:
virtual void RunIteration(
@@ -31,7 +31,7 @@
const control_loops::AngleAdjustLoop::Position *position,
::aos::control_loops::Output *output,
::aos::control_loops::Status *status);
-
+
private:
// Allows the testing code to access some of private members.
friend class testing::AngleAdjustTest_RezeroWithMissingPos_Test;
@@ -41,9 +41,10 @@
// turns on/off recording data.
static const bool testing = true;
- static constexpr double dt = 0.01;
+ // The time step of the control loop.
+ static const double dt;
- // Fetches and locally caches the latest set of constants.
+ // Fetches and locally caches the latest set of constants.
// Returns whether it succeeded or not.
bool FetchConstants();
@@ -61,10 +62,10 @@
int error_count_;
// Local cache of angle adjust geometry constants.
- double horizontal_lower_limit_;
- double horizontal_upper_limit_;
- ::std::array<double, 2> horizontal_hall_effect_stop_angle_;
- double horizontal_zeroing_speed_;
+ double lower_limit_;
+ double upper_limit_;
+ ::std::array<double, 2> hall_effect_stop_angle_;
+ double zeroing_speed_;
// Stores information from the queue about the hall effect sensors.
// Because we don't have arrays in the queue, we needs these to convert
@@ -81,4 +82,4 @@
} // namespace control_loops
} // namespace frc971
-#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_H_
+#endif // FRC971_CONTROL_LOOPS_ANGLE_ADJUST_ANGLE_ADJUST_H_
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
index 749f905..242f3ce 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
+++ b/frc971/control_loops/angle_adjust/angle_adjust_lib_test.cc
@@ -19,14 +19,15 @@
namespace testing {
-// Class which simulates the angle_adjust and sends out queue messages containing the
-// position.
+// Class which simulates the angle_adjust and
+// sends out queue messages containing the position.
class AngleAdjustMotorSimulation {
public:
// Constructs a motor simulation. initial_position is the inital angle of the
// angle_adjust, which will be treated as 0 by the encoder.
- AngleAdjustMotorSimulation(double initial_position)
- : angle_adjust_plant_(new StateFeedbackPlant<2, 1, 1>(MakeAngleAdjustPlant())),
+ explicit AngleAdjustMotorSimulation(double initial_position)
+ : angle_adjust_plant_(
+ new StateFeedbackPlant<2, 1, 1>(MakeAngleAdjustPlant())),
my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
".frc971.control_loops.angle_adjust.position",
@@ -60,28 +61,28 @@
void SendPositionMessage() {
const double angle = GetPosition();
- ::std::array<double, 2> horizontal_hall_effect_start_angle;
- ASSERT_TRUE(constants::angle_adjust_horizontal_hall_effect_start_angle(
- &horizontal_hall_effect_start_angle));
- ::std::array<double, 2> horizontal_hall_effect_stop_angle;
- ASSERT_TRUE(constants::angle_adjust_horizontal_hall_effect_stop_angle(
- &horizontal_hall_effect_stop_angle));
+ ::std::array<double, 2> hall_effect_start_angle;
+ ASSERT_TRUE(constants::angle_adjust_hall_effect_start_angle(
+ &hall_effect_start_angle));
+ ::std::array<double, 2> hall_effect_stop_angle;
+ ASSERT_TRUE(constants::angle_adjust_hall_effect_stop_angle(
+ &hall_effect_stop_angle));
::aos::ScopedMessagePtr<control_loops::AngleAdjustLoop::Position> position =
my_angle_adjust_loop_.position.MakeMessage();
- position->before_angle = angle;
+ position->bottom_angle = angle;
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
double abs_position = GetAbsolutePosition();
- if (abs_position >= horizontal_hall_effect_start_angle[0] &&
- abs_position <= horizontal_hall_effect_stop_angle[0]) {
+ if (abs_position >= hall_effect_start_angle[0] &&
+ abs_position <= hall_effect_stop_angle[0]) {
position->bottom_hall_effect = true;
} else {
position->bottom_hall_effect = false;
}
- if (abs_position >= horizontal_hall_effect_start_angle[1] &&
- abs_position <= horizontal_hall_effect_stop_angle[1]) {
+ if (abs_position >= hall_effect_start_angle[1] &&
+ abs_position <= hall_effect_stop_angle[1]) {
position->middle_hall_effect = true;
} else {
position->middle_hall_effect = false;
@@ -89,13 +90,13 @@
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- if ((last_position_ < horizontal_hall_effect_start_angle[0] ||
- last_position_ > horizontal_hall_effect_stop_angle[0]) &&
+ if ((last_position_ < hall_effect_start_angle[0] ||
+ last_position_ > hall_effect_stop_angle[0]) &&
(position->bottom_hall_effect)) {
calibration_value_[0] = -initial_position_;
}
- if ((last_position_ < horizontal_hall_effect_start_angle[1] ||
- last_position_ > horizontal_hall_effect_stop_angle[1]) &&
+ if ((last_position_ < hall_effect_start_angle[1] ||
+ last_position_ > hall_effect_stop_angle[1]) &&
(position->middle_hall_effect)) {
calibration_value_[1] = -initial_position_;
}
@@ -113,18 +114,19 @@
angle_adjust_plant_->Update();
// Assert that we are in the right physical range.
- double horizontal_upper_physical_limit;
- ASSERT_TRUE(constants::angle_adjust_horizontal_upper_physical_limit(
- &horizontal_upper_physical_limit));
- double horizontal_lower_physical_limit;
- ASSERT_TRUE(constants::angle_adjust_horizontal_lower_physical_limit(
- &horizontal_lower_physical_limit));
+ double upper_physical_limit;
+ ASSERT_TRUE(constants::angle_adjust_upper_physical_limit(
+ &upper_physical_limit));
+ double lower_physical_limit;
+ ASSERT_TRUE(constants::angle_adjust_lower_physical_limit(
+ &lower_physical_limit));
- EXPECT_GE(horizontal_upper_physical_limit, angle_adjust_plant_->Y(0, 0));
- EXPECT_LE(horizontal_lower_physical_limit, angle_adjust_plant_->Y(0, 0));
+ EXPECT_GE(upper_physical_limit, angle_adjust_plant_->Y(0, 0));
+ EXPECT_LE(lower_physical_limit, angle_adjust_plant_->Y(0, 0));
}
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> angle_adjust_plant_;
+
private:
AngleAdjustLoop my_angle_adjust_loop_;
double initial_position_;
@@ -145,13 +147,14 @@
AngleAdjustMotor angle_adjust_motor_;
AngleAdjustMotorSimulation angle_adjust_motor_plant_;
- AngleAdjustTest() : my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
- 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
- ".frc971.control_loops.angle_adjust.position",
- ".frc971.control_loops.angle_adjust.output",
- ".frc971.control_loops.angle_adjust.status"),
- angle_adjust_motor_(&my_angle_adjust_loop_),
- angle_adjust_motor_plant_(.75) {
+ AngleAdjustTest() :
+ my_angle_adjust_loop_(".frc971.control_loops.angle_adjust",
+ 0x65c7ef53, ".frc971.control_loops.angle_adjust.goal",
+ ".frc971.control_loops.angle_adjust.position",
+ ".frc971.control_loops.angle_adjust.output",
+ ".frc971.control_loops.angle_adjust.status"),
+ angle_adjust_motor_(&my_angle_adjust_loop_),
+ angle_adjust_motor_plant_(.75) {
// Flush the robot state queue so we can use clean shared memory for this
// test.
::aos::robot_state.Clear();
@@ -190,7 +193,7 @@
VerifyNearGoal();
}
-// Tests that the angle_adjust zeros correctly starting on the hall effect sensor.
+// Tests that the angle_adjust zeros correctly starting on the sensor.
TEST_F(AngleAdjustTest, ZerosStartingOn) {
angle_adjust_motor_plant_.Reinitialize(0.25);
my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.1).Send();
@@ -229,7 +232,7 @@
} else {
if (i > 310) {
// Should be re-zeroing now.
- EXPECT_EQ(HallEffectLoop<2>::UNINITIALIZED,
+ EXPECT_EQ(HallEffectLoop<2>::UNINITIALIZED,
angle_adjust_motor_.hall_effect_.state_);
}
my_angle_adjust_loop_.goal.MakeWithBuilder().goal(0.2).Send();
@@ -237,7 +240,7 @@
if (i == 430) {
EXPECT_TRUE(
HallEffectLoop<2>::ZEROING == angle_adjust_motor_.hall_effect_.state_
- || HallEffectLoop<2>::MOVING_OFF ==
+ || HallEffectLoop<2>::MOVING_OFF ==
angle_adjust_motor_.hall_effect_.state_);
}
@@ -305,7 +308,6 @@
VerifyNearGoal();
}
-
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/angle_adjust/angle_adjust_motor.q b/frc971/control_loops/angle_adjust/angle_adjust_motor.q
index 2a7c562..eac3ada 100644
--- a/frc971/control_loops/angle_adjust/angle_adjust_motor.q
+++ b/frc971/control_loops/angle_adjust/angle_adjust_motor.q
@@ -7,8 +7,8 @@
message Position {
// Angle of the encoder.
- double before_angle;
- double after_angle;
+ double bottom_angle;
+ double middle_angle;
bool bottom_hall_effect;
bool middle_hall_effect;
// The exact position when the corresponding hall_effect changed.
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index d176f63..db77a2c 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -50,53 +50,6 @@
],
},
{
- 'target_name': 'wrist_lib',
- 'type': 'static_library',
- 'sources': [
- 'wrist.cc',
- 'wrist_motor_plant.cc',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
- 'control_loops',
- '<(AOS)/common/common.gyp:controls',
- '<(DEPTH)/frc971/frc971.gyp:common',
- 'state_feedback_loop',
- ],
- 'export_dependent_settings': [
- 'state_feedback_loop',
- '<(AOS)/common/common.gyp:controls',
- 'control_loops',
- ],
- },
- {
- 'target_name': 'wrist_lib_test',
- 'type': 'executable',
- 'sources': [
- 'wrist_lib_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- '<(AOS)/build/aos.gyp:libaos',
- 'control_loops',
- 'wrist_lib',
- '<(AOS)/common/common.gyp:queue_testutils',
- 'state_feedback_loop',
- ],
- },
- {
- 'target_name': 'wrist',
- 'type': 'executable',
- 'sources': [
- 'wrist_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:libaos',
- 'wrist_lib',
- 'control_loops',
- ],
- },
- {
'target_name': 'DriveTrain',
'type': 'executable',
'sources': [
diff --git a/frc971/control_loops/hall_effect_loop-inl.h b/frc971/control_loops/hall_effect_loop-inl.h
index f5deb67..29c4273 100644
--- a/frc971/control_loops/hall_effect_loop-inl.h
+++ b/frc971/control_loops/hall_effect_loop-inl.h
@@ -59,15 +59,17 @@
template <int kNumHallEffect>
void HallEffectLoop<kNumHallEffect>::LimitZeroingGoal() {
- if (loop_->U_uncapped(0, 0) > kMaxZeroingVoltage) {
- double excess = (loop_->U_uncapped(0, 0) - kMaxZeroingVoltage)
- / loop_->K(0, 0);
- zeroing_position_ -= excess;
- }
- if (loop_->U_uncapped(0, 0) < -kMaxZeroingVoltage) {
- double excess = (loop_->U_uncapped(0, 0) + kMaxZeroingVoltage)
- / loop_->K(0, 0);
- zeroing_position_ -= excess;
+ if (state_ == MOVING_OFF || state_ == ZEROING) {
+ if (loop_->U_uncapped(0, 0) > kMaxZeroingVoltage) {
+ double excess = (loop_->U_uncapped(0, 0) - kMaxZeroingVoltage)
+ / loop_->K(0, 0);
+ zeroing_position_ -= excess;
+ }
+ if (loop_->U_uncapped(0, 0) < -kMaxZeroingVoltage) {
+ double excess = (loop_->U_uncapped(0, 0) + kMaxZeroingVoltage)
+ / loop_->K(0, 0);
+ zeroing_position_ -= excess;
+ }
}
}
@@ -182,10 +184,6 @@
return;
}
- if (state_ == MOVING_OFF || state_ == ZEROING) {
- LimitZeroingGoal();
- }
-
if (good_position) {
LOG(DEBUG,
"calibration sensor: %d zero_offset: %f absolute_position: %f\n",