Switched to 5ms cycles on the control loops.
Change-Id: I1aae3b30a9c422f1920ccb1c15e035ae847f85a9
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index 29682f2..621c2d4 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -42,7 +42,7 @@
voltage_ = std::max(-max_voltage_, voltage_);
mutable_U(0, 0) = voltage_ - old_voltage;
- LOG_STRUCT(DEBUG, "output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
+ LOG_STRUCT(DEBUG, "shooter_output", ShooterVoltageToLog(X_hat(2, 0), voltage_));
last_voltage_ = voltage_;
capped_goal_ = false;
@@ -402,8 +402,11 @@
}
if (load_timeout_ < Time::Now()) {
if (position) {
- if (!position->pusher_distal.current ||
- !position->pusher_proximal.current) {
+ // If none of the sensors is triggered, estop.
+ // Otherwise, trigger anyways if it has been 0.5 seconds more.
+ if (!(position->pusher_distal.current ||
+ position->pusher_proximal.current) ||
+ (load_timeout_ + Time::InSeconds(0.5) < Time::Now()) {
state_ = STATE_ESTOP;
LOG(ERROR, "Estopping because took too long to load.\n");
}
@@ -534,7 +537,7 @@
// Also move on if it times out.
if (((::std::abs(firing_starting_position_ -
shooter_.absolute_position()) > 0.0005 &&
- cycles_not_moved_ > 3) ||
+ cycles_not_moved_ > 6) ||
Time::Now() > shot_end_time_) &&
::aos::robot_state->voltage_battery > 10.5) {
state_ = STATE_REQUEST_LOAD;
@@ -679,6 +682,10 @@
last_proximal_current_ = position->pusher_proximal.current;
}
+ status->absolute_position = shooter_.absolute_position();
+ status->absolute_velocity = shooter_.absolute_velocity();
+ status->state = state_;
+
status->shots = shot_count_;
}
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index 0efbbd0..93b45e0 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -114,7 +114,7 @@
};
const Time kUnloadTimeout = Time::InSeconds(10);
-const Time kLoadTimeout = Time::InSeconds(2);
+const Time kLoadTimeout = Time::InSeconds(2.0);
const Time kLoadProblemEndTimeout = Time::InSeconds(1.0);
const Time kShooterBrakeSetTime = Time::InSeconds(0.05);
// Time to wait after releasing the latch piston before winching back again.
diff --git a/y2014/control_loops/shooter/shooter.q b/y2014/control_loops/shooter/shooter.q
index 009e20e..eaaaa2e 100644
--- a/y2014/control_loops/shooter/shooter.q
+++ b/y2014/control_loops/shooter/shooter.q
@@ -49,6 +49,10 @@
// What we think the current position of the hard stop on the shooter is, in
// shot power (Joules).
double hard_stop_power;
+
+ double absolute_position;
+ double absolute_velocity;
+ uint32_t state;
};
queue Goal goal;