Merge "Fixed DMA on the roboRIO."
diff --git a/aos/common/util/phased_loop.h b/aos/common/util/phased_loop.h
index d231449..f3c417a 100644
--- a/aos/common/util/phased_loop.h
+++ b/aos/common/util/phased_loop.h
@@ -10,8 +10,6 @@
// Will not be accurate if ms isn't a factor of 1000.
// offset is in us.
void PhasedLoopXMS(int ms, int offset);
-// offset is in us.
-inline void PhasedLoop10MS(int offset) { PhasedLoopXMS(10, offset); }
} // namespace time
} // namespace aos
diff --git a/frc971/actions/drivetrain_actor.cc b/frc971/actions/drivetrain_actor.cc
index 71a1be8..a453502 100644
--- a/frc971/actions/drivetrain_actor.cc
+++ b/frc971/actions/drivetrain_actor.cc
@@ -45,9 +45,7 @@
2.0);
while (true) {
- // wait until next 10ms tick
- // TODO(sensors): update this time thing for some reason.
- ::aos::time::PhasedLoop10MS(5000);
+ ::aos::time::PhasedLoopXMS(5, 2500);
control_loops::drivetrain_queue.status.FetchLatest();
if (control_loops::drivetrain_queue.status.get()) {
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 5d9733a..a5cfd22 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -74,7 +74,7 @@
const double side_offset = kRobotWidth * radians / 2.0;
while (true) {
- ::aos::time::PhasedLoop10MS(5000); // wait until next 10ms tick
+ ::aos::time::PhasedLoopXMS(5, 2500);
driveTrainState = profile.Update(side_offset, goal_velocity);
if (::std::abs(driveTrainState(0, 0) - side_offset) < epsilon) break;
@@ -100,7 +100,7 @@
void WaitUntilDoneOrCanceled(aos::common::actions::Action *action) {
while (true) {
// Poll the running bit and auto done bits.
- ::aos::time::PhasedLoop10MS(5000);
+ ::aos::time::PhasedLoopXMS(5, 2500);
if (!action->Running() || ShouldExitAuto()) {
return;
}
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 8807baf..3347da9 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -285,6 +285,8 @@
right_elevator_offset_ = -position->elevator.right.encoder;
left_arm_offset_ = -position->arm.left.encoder;
right_arm_offset_ = -position->arm.right.encoder;
+ elevator_loop_->mutable_X_hat().setZero();
+ arm_loop_->mutable_X_hat().setZero();
LOG(INFO, "Initializing arm offsets to %f, %f\n", left_arm_offset_,
right_arm_offset_);
LOG(INFO, "Initializing elevator offsets to %f, %f\n",
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index e306553..8329220 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -27,8 +27,6 @@
namespace input {
namespace joysticks {
-const ButtonLocation kDriveControlLoopEnable1(1, 7),
- kDriveControlLoopEnable2(1, 11);
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
@@ -36,9 +34,7 @@
class Reader : public ::aos::input::JoystickInput {
public:
- Reader()
- : is_high_gear_(false),
- was_running_(false) {}
+ Reader() : was_running_(false) {}
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
bool last_auto_running = auto_running_;
@@ -58,70 +54,17 @@
}
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- bool is_control_loop_driving = false;
- double left_goal = 0.0;
- double right_goal = 0.0;
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
- const double kThrottleGain = 1.0 / 2.5;
- if (false && (data.IsPressed(kDriveControlLoopEnable1) ||
- data.IsPressed(kDriveControlLoopEnable2))) {
- // TODO(austin): Static sucks!
- static double distance = 0.0;
- static double angle = 0.0;
- static double filtered_goal_distance = 0.0;
- if (data.PosEdge(kDriveControlLoopEnable1) ||
- data.PosEdge(kDriveControlLoopEnable2)) {
- if (drivetrain_queue.position.FetchLatest() &&
- gyro_reading.FetchLatest()) {
- distance = (drivetrain_queue.position->left_encoder +
- drivetrain_queue.position->right_encoder) /
- 2.0 -
- throttle * kThrottleGain / 2.0;
- angle = gyro_reading->angle;
- filtered_goal_distance = distance;
- }
- }
- is_control_loop_driving = true;
- // const double gyro_angle = Gyro.View().angle;
- const double goal_theta = angle - wheel * 0.27;
- const double goal_distance = distance + throttle * kThrottleGain;
- const double robot_width = 22.0 / 100.0 * 2.54;
- const double kMaxVelocity = 0.6;
- if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
- filtered_goal_distance += kMaxVelocity * 0.02;
- } else if (goal_distance <
- -kMaxVelocity * 0.02 + filtered_goal_distance) {
- filtered_goal_distance -= kMaxVelocity * 0.02;
- } else {
- filtered_goal_distance = goal_distance;
- }
- left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
- right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
- is_high_gear_ = false;
-
- LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
- }
if (!drivetrain_queue.goal.MakeWithBuilder()
.steering(wheel)
.throttle(throttle)
- //.highgear(is_high_gear_)
.quickturn(data.IsPressed(kQuickTurn))
- .control_loop_driving(is_control_loop_driving)
- .left_goal(left_goal)
- .right_goal(right_goal)
- .left_velocity_goal(0)
- .right_velocity_goal(0)
+ .control_loop_driving(false)
.Send()) {
LOG(WARNING, "sending stick values failed\n");
}
- if (data.PosEdge(kShiftHigh)) {
- is_high_gear_ = false;
- }
- if (data.PosEdge(kShiftLow)) {
- is_high_gear_ = true;
- }
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
@@ -146,12 +89,10 @@
::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
}
- bool is_high_gear_;
bool was_running_;
-
bool auto_running_ = false;
- aos::common::actions::ActionQueue action_queue_;
+ ::aos::common::actions::ActionQueue action_queue_;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,