got all 3 auto modes working (!!!)
diff --git a/aos/common/util/trapezoid_profile.cc b/aos/common/util/trapezoid_profile.cc
index 9035f89..4c5f07d 100644
--- a/aos/common/util/trapezoid_profile.cc
+++ b/aos/common/util/trapezoid_profile.cc
@@ -2,6 +2,8 @@
#include <assert.h>
+#include "aos/common/logging/logging.h"
+
using ::Eigen::Matrix;
namespace aos {
diff --git a/aos/common/util/util.gyp b/aos/common/util/util.gyp
index 682db82..b20a7d9 100644
--- a/aos/common/util/util.gyp
+++ b/aos/common/util/util.gyp
@@ -55,6 +55,7 @@
'dependencies': [
'<(EXTERNALS):eigen',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/build/aos.gyp:logging',
],
'export_dependent_settings': [
'<(EXTERNALS):eigen',
diff --git a/frc971/actions/action.h b/frc971/actions/action.h
index 5b23d61..591aac1 100644
--- a/frc971/actions/action.h
+++ b/frc971/actions/action.h
@@ -31,6 +31,7 @@
if (initially_running != 0) {
while (action_q_->goal->run == initially_running) {
LOG(INFO, "run is still %" PRIx32 "\n", initially_running);
+ action_q_->goal.FetchNextBlocking();
}
}
} else {
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
index 07a2abb..0d95b91 100644
--- a/frc971/actions/drivetrain_action.cc
+++ b/frc971/actions/drivetrain_action.cc
@@ -43,11 +43,6 @@
while (true) {
// wait until next 10ms tick
::aos::time::PhasedLoop10MS(5000);
- 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;
control_loops::drivetrain.status.FetchLatest();
if (control_loops::drivetrain.status.get()) {
@@ -55,13 +50,14 @@
if (::std::abs(status.uncapped_left_voltage -
status.uncapped_right_voltage) >
24) {
+ LOG(DEBUG, "spinning in place\n");
// They're more than 24V apart, so stop moving forwards and let it deal
// with spinning first.
profile.SetGoal(
(status.filtered_left_position + status.filtered_right_position) /
2.0);
} else {
- static const double divisor = K(0, 1) + K(0, 3);
+ static const double divisor = K(0, 0) + K(0, 2);
double dx_left, dx_right;
if (status.uncapped_left_voltage > 12.0) {
dx_left = (status.uncapped_left_voltage - 12.0) / divisor;
@@ -99,6 +95,12 @@
LOG(FATAL, "no drivetrain status!\n");
}
+ 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(drive_profile_goal_state(0, 0) - yoffset) < epsilon &&
::std::abs(turn_profile_goal_state(0, 0) - turn_offset) < epsilon) {
break;
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 32f6979..aaf0d2b 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -57,6 +57,10 @@
.highgear(false)
.steering(0.0)
.throttle(0.0)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
.Send();
}
@@ -232,7 +236,7 @@
void ResetCounts() {
hot_goal.FetchLatest();
if (hot_goal.get()) {
- memcpy(&start_counts_, hot_goal.get(), sizeof(start_counts_));
+ start_counts_ = *hot_goal;
LOG_STRUCT(INFO, "counts reset to", start_counts_);
start_counts_valid_ = true;
} else {
@@ -241,11 +245,25 @@
}
}
- void Update() {
- hot_goal.FetchLatest();
+ void Update(bool block = false) {
+ if (block) {
+ hot_goal.FetchAnother();
+ } else {
+ hot_goal.FetchLatest();
+ }
if (hot_goal.get()) LOG_STRUCT(INFO, "new counts", *hot_goal);
}
+ bool left_triggered() const {
+ if (!start_counts_valid_ || !hot_goal.get()) return false;
+ return (hot_goal->left_count - start_counts_.left_count) > kThreshold;
+ }
+
+ bool right_triggered() const {
+ if (!start_counts_valid_ || !hot_goal.get()) return false;
+ return (hot_goal->right_count - start_counts_.right_count) > kThreshold;
+ }
+
bool is_left() const {
if (!start_counts_valid_ || !hot_goal.get()) return false;
const uint64_t left_difference =
@@ -254,6 +272,7 @@
hot_goal->right_count - start_counts_.right_count;
if (left_difference > kThreshold) {
if (right_difference > kThreshold) {
+ // We've seen a lot of both, so pick the one we've seen the most of.
return left_difference > right_difference;
} else {
// We've seen enough left but not enough right, so go with it.
@@ -273,6 +292,7 @@
hot_goal->right_count - start_counts_.right_count;
if (right_difference > kThreshold) {
if (left_difference > kThreshold) {
+ // We've seen a lot of both, so pick the one we've seen the most of.
return right_difference > left_difference;
} else {
// We've seen enough right but not enough left, so go with it.
@@ -285,7 +305,7 @@
}
private:
- static const uint64_t kThreshold = 10;
+ static const uint64_t kThreshold = 5;
::frc971::HotGoal start_counts_;
bool start_counts_valid_;
@@ -295,6 +315,7 @@
enum class AutoVersion : uint8_t {
kStraight,
kDoubleHot,
+ kSingleHot,
};
// The front of the robot is 1.854 meters from the wall
@@ -311,9 +332,17 @@
LOG(WARNING, "not sure which auto mode to use\n");
auto_version = AutoVersion::kStraight;
} else {
- auto_version =
- (::frc971::sensors::auto_mode->voltage > 2.5) ? AutoVersion::kDoubleHot
- : AutoVersion::kStraight;
+ static const double kSelectorMin = 0.2, kSelectorMax = 4.4;
+
+ const double kSelectorStep = (kSelectorMax - kSelectorMin) / 3.0;
+ if (::frc971::sensors::auto_mode->voltage < kSelectorStep + kSelectorMin) {
+ auto_version = AutoVersion::kSingleHot;
+ } else if (::frc971::sensors::auto_mode->voltage <
+ 2 * kSelectorStep + kSelectorMin) {
+ auto_version = AutoVersion::kStraight;
+ } else {
+ auto_version = AutoVersion::kDoubleHot;
+ }
}
LOG(INFO, "running auto %" PRIu8 "\n", auto_version);
@@ -369,6 +398,13 @@
SetDriveGoal(0, 2, first_shot_left ? kTurnAngle : -kTurnAngle);
WaitUntilDoneOrCanceled(drivetrain_action.get());
if (ShouldExitAuto()) return;
+ } else if (auto_version == AutoVersion::kSingleHot) {
+ do {
+ hot_goal_decoder.Update(true);
+ if (ShouldExitAuto()) return;
+ } while (!hot_goal_decoder.left_triggered() &&
+ (::aos::time::Time::Now() - start_time) <
+ ::aos::time::Time::InSeconds(9));
}
// Shoot.
@@ -383,10 +419,13 @@
SetDriveGoal(0, 2, first_shot_left ? -kTurnAngle : kTurnAngle);
WaitUntilDoneOrCanceled(drivetrain_action.get());
if (ShouldExitAuto()) return;
+ } else if (auto_version == AutoVersion::kSingleHot) {
+ LOG(INFO, "auto done at %f\n",
+ (::aos::time::Time::Now() - start_time).ToSeconds());
+ PositionClawVertically(0.0, 0.0);
+ return;
}
- hot_goal_decoder.ResetCounts();
-
{
if (ShouldExitAuto()) return;
// Intake the new ball.
@@ -411,6 +450,7 @@
auto drivetrain_action =
SetDriveGoal(-(kShootDistance + kPickupDistance), 2.5);
time::SleepFor(time::Time::InSeconds(0.3));
+ hot_goal_decoder.ResetCounts();
if (ShouldExitAuto()) return;
PositionClawForShot();
LOG(INFO, "Waiting until drivetrain is finished\n");
diff --git a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index b254fd7..c33562d 100644
--- a/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -73,7 +73,7 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.46584167586, 0.0410810558113, 41.9788073691, 4.62131736499, 0.0410810558113, 1.46584167586, 4.62131736499, 41.9788073691;
+ L << 1.03584167586, 0.0410810558113, 17.1117704011, 3.22861251708, 0.0410810558113, 1.03584167586, 3.22861251708, 17.1117704011;
Eigen::Matrix<double, 2, 4> K;
K << 128.210620632, 6.93828382074, 5.1103668677, 0.729493080206, 5.1103668677, 0.729493080206, 128.210620632, 6.93828382074;
return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowLowPlantCoefficients());
@@ -81,7 +81,7 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.45856045663, 0.0145216082167, 41.3651523316, 1.69047524315, 0.0145216082167, 1.65219224519, 3.02153581754, 64.1990849762;
+ L << 1.02891982345, 0.0143715516939, 16.6997472571, 1.23741823594, 0.0143715516939, 1.22183287838, 2.40440177527, 33.5403677132;
Eigen::Matrix<double, 2, 4> K;
K << 127.841025245, 6.90618982868, -2.11442482189, 0.171361719101, 11.257083857, 1.47190974842, 138.457761234, 11.0770574926;
return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowHighPlantCoefficients());
@@ -89,7 +89,7 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.64511269416, 0.045918446918, 63.7288738736, 5.55411342044, 0.045918446918, 1.46564000767, 3.81771765442, 41.8737588606;
+ L << 1.21584032636, 0.045928553155, 33.3376290177, 4.12652814156, 0.045928553155, 1.03491237546, 2.45838080322, 16.967272239;
Eigen::Matrix<double, 2, 4> K;
K << 138.457761234, 11.0770574926, 11.257083857, 1.47190974842, -2.11442482191, 0.171361719101, 127.841025245, 6.90618982869;
return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighLowPlantCoefficients());
@@ -97,7 +97,7 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.64543980657, 0.0146814193986, 63.6243330534, 1.89166687195, 0.0146814193986, 1.64543980657, 1.89166687195, 63.6243330534;
+ L << 1.21543980657, 0.0146814193986, 33.1557840927, 1.47278696694, 0.0146814193986, 1.21543980657, 1.47278696694, 33.1557840927;
Eigen::Matrix<double, 2, 4> K;
K << 138.52410152, 11.0779399816, 3.96842371774, 0.882728086516, 3.96842371774, 0.882728086517, 138.52410152, 11.0779399816;
return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighHighPlantCoefficients());
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 910196e..1a1bbe7 100755
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -143,8 +143,8 @@
print self.K
print numpy.linalg.eig(self.A - self.B * self.K)[0]
- self.hlp = 0.12
- self.llp = 0.15
+ self.hlp = 0.3
+ self.llp = 0.4
self.PlaceObserverPoles([self.hlp, self.hlp, self.llp, self.llp])
self.U_max = numpy.matrix([[12.0], [12.0]])
diff --git a/frc971/input/hot_goal_reader.cc b/frc971/input/hot_goal_reader.cc
index f42f370..cc6edfc 100644
--- a/frc971/input/hot_goal_reader.cc
+++ b/frc971/input/hot_goal_reader.cc
@@ -15,6 +15,17 @@
int main() {
::aos::InitNRT();
+ uint64_t left_count, right_count;
+ ::frc971::hot_goal.FetchLatest();
+ if (::frc971::hot_goal.get()) {
+ LOG_STRUCT(DEBUG, "starting with", *::frc971::hot_goal);
+ left_count = ::frc971::hot_goal->left_count;
+ right_count = ::frc971::hot_goal->left_count;
+ } else {
+ LOG(DEBUG, "no starting message\n");
+ left_count = right_count = 0;
+ }
+
int my_socket = -1;
while (true) {
if (my_socket == -1) {
@@ -58,8 +69,8 @@
}
LOG(INFO, "accepted (is %d)\n", connection);
- fd_set fds;
while (connection != -1) {
+ fd_set fds;
FD_ZERO(&fds);
FD_SET(connection, &fds);
struct timeval timeout_timeval =
@@ -74,7 +85,6 @@
sizeof(data));
break;
}
- static uint64_t left_count = 0, right_count = 0;
if (data & 0x01) ++right_count;
if (data & 0x02) ++left_count;
auto message = ::frc971::hot_goal.MakeMessage();