Fixed Austin's comments.
Squashed 'Changed fender shot to long without IMU' and
fixed Austin's comments.
Change-Id: Id81f4f8d35bb399d33bb74bd3d4e895603817711
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index d2d1375..fcfe33c 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -191,10 +191,6 @@
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() &&
@@ -504,6 +500,10 @@
double goal_angle_;
double separation_angle_, shot_separation_angle_;
double velocity_compensation_;
+ // Distance, angle, and filtered goal for closed loop driving.
+ double distance;
+ double angle;
+ double filtered_goal_distance;
double intake_power_;
bool was_running_;
bool moving_for_shot_ = false;
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
index 86f78dc..22610ae 100644
--- a/y2014_bot3/joystick_reader.cc
+++ b/y2014_bot3/joystick_reader.cc
@@ -66,8 +66,6 @@
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
bool is_control_loop_driving = false;
- static double left_goal = 0.0;
- static double right_goal = 0.0;
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
@@ -139,6 +137,9 @@
bool auto_running_ = false;
bool is_high_gear_;
+ // Turning goals.
+ double left_goal;
+ double right_goal;
::aos::util::SimpleLogInterval no_drivetrain_status_ =
::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
diff --git a/y2016/actors/BUILD b/y2016/actors/BUILD
index 453d49e..cfbeb2b 100644
--- a/y2016/actors/BUILD
+++ b/y2016/actors/BUILD
@@ -5,60 +5,12 @@
filegroup(
name = 'binaries',
srcs = [
- ':drivetrain_action',
':superstructure_action',
':autonomous_action',
],
)
queue_library(
- name = 'drivetrain_action_queue',
- srcs = [
- 'drivetrain_action.q',
- ],
- deps = [
- '//aos/common/actions:action_queue',
- ],
-)
-
-cc_library(
- name = 'drivetrain_action_lib',
- srcs = [
- 'drivetrain_actor.cc',
- ],
- hdrs = [
- 'drivetrain_actor.h',
- ],
- deps = [
- ':drivetrain_action_queue',
- '//aos/common:time',
- '//aos/common:math',
- '//aos/common/util:phased_loop',
- '//aos/common/logging',
- '//aos/common/actions:action_lib',
- '//aos/common/logging:queue_logging',
- '//aos/common/util:trapezoid_profile',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//frc971/control_loops:state_feedback_loop',
- '//third_party/eigen',
- '//y2016:constants',
- '//y2016/control_loops/drivetrain:polydrivetrain_plants',
- ],
-)
-
-cc_binary(
- name = 'drivetrain_action',
- srcs = [
- 'drivetrain_actor_main.cc',
- ],
- deps = [
- ':drivetrain_action_lib',
- ':drivetrain_action_queue',
- '//aos/linux_code:init',
- ],
-)
-
-queue_library(
name = 'superstructure_action_queue',
srcs = [
'superstructure_action.q',
diff --git a/y2016/actors/drivetrain_action.q b/y2016/actors/drivetrain_action.q
deleted file mode 100644
index 9bbebb2..0000000
--- a/y2016/actors/drivetrain_action.q
+++ /dev/null
@@ -1,29 +0,0 @@
-package y2016.actors;
-
-import "aos/common/actions/actions.q";
-
-// Parameters to send with start.
-struct DrivetrainActionParams {
- double left_initial_position;
- double right_initial_position;
- double y_offset;
- double theta_offset;
- double maximum_velocity;
- double maximum_acceleration;
- double maximum_turn_velocity;
- double maximum_turn_acceleration;
-};
-
-queue_group DrivetrainActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- DrivetrainActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
-
-queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/y2016/actors/drivetrain_actor.cc b/y2016/actors/drivetrain_actor.cc
deleted file mode 100644
index 2c7c0e5..0000000
--- a/y2016/actors/drivetrain_actor.cc
+++ /dev/null
@@ -1,182 +0,0 @@
-#include "y2016/actors/drivetrain_actor.h"
-
-#include <functional>
-#include <numeric>
-
-#include <Eigen/Dense>
-
-#include "aos/common/util/phased_loop.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/util/trapezoid_profile.h"
-#include "aos/common/commonmath.h"
-#include "aos/common/time.h"
-
-#include "y2016/actors/drivetrain_actor.h"
-#include "y2016/constants.h"
-#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-
-namespace y2016 {
-namespace actors {
-
-DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup *s)
- : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
- loop_(::y2016::control_loops::drivetrain::MakeDrivetrainLoop()) {
- loop_.set_controller_index(3);
-}
-
-bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = loop_.K();
-
- const double yoffset = params.y_offset;
- const double turn_offset =
- params.theta_offset * control_loops::drivetrain::kRobotRadius;
- 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(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
- 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(params.maximum_acceleration);
- profile.set_maximum_velocity(params.maximum_velocity);
- turn_profile.set_maximum_acceleration(
- params.maximum_turn_acceleration *
- control_loops::drivetrain::kRobotRadius);
- turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- control_loops::drivetrain::kRobotRadius);
-
- while (true) {
- ::aos::time::PhasedLoopXMS(5, 2500);
-
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
- if (::frc971::control_loops::drivetrain_queue.status.get()) {
- const auto &status = *::frc971::control_loops::drivetrain_queue.status;
- 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.estimated_left_position + status.estimated_right_position -
- params.left_initial_position - params.right_initial_position) /
- 2.0);
- } else {
- 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;
- } else if (status.uncapped_left_voltage < -12.0) {
- dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
- } else {
- dx_left = 0;
- }
-
- if (status.uncapped_right_voltage > 12.0) {
- dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
- } else if (status.uncapped_right_voltage < -12.0) {
- dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
- } else {
- dx_right = 0;
- }
-
- double dx;
-
- if (dx_left == 0 && dx_right == 0) {
- dx = 0;
- } else if (dx_left != 0 && dx_right != 0 &&
- ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
- // Both saturating in opposite directions. Don't do anything.
- LOG(DEBUG, "Saturating opposite ways, not adjusting\n");
- dx = 0;
- } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
- dx = dx_left;
- } else {
- dx = dx_right;
- }
-
- if (dx != 0) {
- LOG(DEBUG, "adjusting goal by %f\n", dx);
- profile.MoveGoal(-dx);
- }
- }
- } else {
- // If we ever get here, that's bad and we should just give up
- LOG(ERROR, "no drivetrain status!\n");
- return false;
- }
-
- 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;
- }
-
- if (ShouldCancel()) return true;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_goal_state(0, 0) + params.left_initial_position,
- right_goal_state(0, 0) + params.right_initial_position);
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(true)
- .left_goal(left_goal_state(0, 0) + params.left_initial_position)
- .right_goal(right_goal_state(0, 0) + params.right_initial_position)
- .left_velocity_goal(left_goal_state(1, 0))
- .right_velocity_goal(right_goal_state(1, 0))
- .Send();
- }
- if (ShouldCancel()) return true;
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
-
- while (!::frc971::control_loops::drivetrain_queue.status.get()) {
- LOG(WARNING,
- "No previous drivetrain status packet, trying to fetch again\n");
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- if (ShouldCancel()) return true;
- }
-
- while (true) {
- if (ShouldCancel()) return true;
- const double kPositionThreshold = 0.05;
-
- const double left_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_left_position -
- (left_goal_state(0, 0) + params.left_initial_position));
- const double right_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_right_position -
- (right_goal_state(0, 0) + params.right_initial_position));
- const double velocity_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->robot_speed);
- if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
- velocity_error < 0.2) {
- break;
- } else {
- LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
- velocity_error);
- }
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- }
-
- LOG(INFO, "Done moving\n");
- return true;
-}
-
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2016::actors::DrivetrainActionParams ¶ms) {
- return ::std::unique_ptr<DrivetrainAction>(
- new DrivetrainAction(&::y2016::actors::drivetrain_action, params));
-}
-
-} // namespace actors
-} // namespace y2016
diff --git a/y2016/actors/drivetrain_actor.h b/y2016/actors/drivetrain_actor.h
deleted file mode 100644
index 0ab3bf2..0000000
--- a/y2016/actors/drivetrain_actor.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef Y2016_ACTORS_DRIVETRAIN_ACTOR_H_
-#define Y2016_ACTORS_DRIVETRAIN_ACTOR_H_
-
-#include <memory>
-
-#include "aos/common/actions/actor.h"
-#include "aos/common/actions/actions.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-
-#include "y2016/actors/drivetrain_action.q.h"
-
-namespace y2016 {
-namespace actors {
-
-class DrivetrainActor
- : public ::aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
- public:
- explicit DrivetrainActor(DrivetrainActionQueueGroup *s);
-
- bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
-
- private:
- StateFeedbackLoop<4, 2, 2> loop_;
-};
-
-typedef ::aos::common::actions::TypedAction<DrivetrainActionQueueGroup>
- DrivetrainAction;
-
-// Makes a new DrivetrainActor action.
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2016::actors::DrivetrainActionParams ¶ms);
-
-} // namespace actors
-} // namespace y2016
-
-#endif
diff --git a/y2016/actors/drivetrain_actor_main.cc b/y2016/actors/drivetrain_actor_main.cc
deleted file mode 100644
index 0fe9e71..0000000
--- a/y2016/actors/drivetrain_actor_main.cc
+++ /dev/null
@@ -1,18 +0,0 @@
-#include <stdio.h>
-
-#include "aos/linux_code/init.h"
-#include "y2016/actors/drivetrain_action.q.h"
-#include "y2016/actors/drivetrain_actor.h"
-
-using ::aos::time::Time;
-
-int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init(-1);
-
- ::y2016::actors::DrivetrainActor drivetrain(
- &::y2016::actors::drivetrain_action);
- drivetrain.Run();
-
- ::aos::Cleanup();
- return 0;
-}
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 45733ff..6a0edf0 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -120,8 +120,6 @@
void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
bool is_control_loop_driving = false;
- static double left_goal = 0.0;
- static double right_goal = 0.0;
const double wheel = -data.GetAxis(kSteeringWheel);
const double throttle = -data.GetAxis(kDriveThrottle);
@@ -238,18 +236,15 @@
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
} else if (data.IsPressed(kBackFender)) {
- // Fender shot back
- shoulder_goal_ = M_PI / 2.0 - 0.2;
- wrist_goal_ = -0.55;
- shooter_velocity_ = 600.0;
+ // Backwards shot no IMU
+ shoulder_goal_ = M_PI / 2.0 - 0.4;
+ wrist_goal_ = -0.62 - 0.02;
+ shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
} else if (data.IsPressed(kFrontFender)) {
- // Forwards shot, higher
+ // Forwards shot no IMU
shoulder_goal_ = M_PI / 2.0 + 0.1;
- wrist_goal_ = M_PI + 0.41 + 0.02 + 0.020;
- if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle;
- }
+ wrist_goal_ = M_PI + 0.41 + 0.02 - 0.005;
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
} else if (data.IsPressed(kExpand) || data.IsPressed(kWinch)) {
@@ -458,6 +453,10 @@
double wrist_goal_;
double shooter_velocity_ = 0.0;
+ // Turning goals
+ double left_goal;
+ double right_goal;
+
bool was_running_ = false;
bool auto_running_ = false;
diff --git a/y2016_bot3/actors/BUILD b/y2016_bot3/actors/BUILD
index 6453f13..aa774a3 100644
--- a/y2016_bot3/actors/BUILD
+++ b/y2016_bot3/actors/BUILD
@@ -5,60 +5,11 @@
filegroup(
name = 'binaries',
srcs = [
- ':drivetrain_action',
':autonomous_action',
],
)
queue_library(
- name = 'drivetrain_action_queue',
- srcs = [
- 'drivetrain_action.q',
- ],
- deps = [
- '//aos/common/actions:action_queue',
- ],
-)
-
-cc_library(
- name = 'drivetrain_action_lib',
- srcs = [
- 'drivetrain_actor.cc',
- ],
- hdrs = [
- 'drivetrain_actor.h',
- ],
- deps = [
- ':drivetrain_action_queue',
- '//aos/common:time',
- '//aos/common:math',
- '//aos/common/util:phased_loop',
- '//aos/common/logging',
- '//aos/common/actions:action_lib',
- '//aos/common/logging:queue_logging',
- '//aos/common/util:trapezoid_profile',
- '//frc971/control_loops/drivetrain:drivetrain_queue',
- '//frc971/control_loops:state_feedback_loop',
- '//third_party/eigen',
- '//y2016_bot3/control_loops/intake:intake_lib',
- '//y2016_bot3/control_loops/drivetrain:drivetrain_base',
- '//y2016_bot3/control_loops/drivetrain:polydrivetrain_plants',
- ],
-)
-
-cc_binary(
- name = 'drivetrain_action',
- srcs = [
- 'drivetrain_actor_main.cc',
- ],
- deps = [
- ':drivetrain_action_lib',
- ':drivetrain_action_queue',
- '//aos/linux_code:init',
- ],
-)
-
-queue_library(
name = 'autonomous_action_queue',
srcs = [
'autonomous_action.q',
diff --git a/y2016_bot3/actors/drivetrain_action.q b/y2016_bot3/actors/drivetrain_action.q
deleted file mode 100644
index 7775cad..0000000
--- a/y2016_bot3/actors/drivetrain_action.q
+++ /dev/null
@@ -1,29 +0,0 @@
-package y2016_bot3.actors;
-
-import "aos/common/actions/actions.q";
-
-// Parameters to send with start.
-struct DrivetrainActionParams {
- double left_initial_position;
- double right_initial_position;
- double y_offset;
- double theta_offset;
- double maximum_velocity;
- double maximum_acceleration;
- double maximum_turn_velocity;
- double maximum_turn_acceleration;
-};
-
-queue_group DrivetrainActionQueueGroup {
- implements aos.common.actions.ActionQueueGroup;
-
- message Goal {
- uint32_t run;
- DrivetrainActionParams params;
- };
-
- queue Goal goal;
- queue aos.common.actions.Status status;
-};
-
-queue_group DrivetrainActionQueueGroup drivetrain_action;
diff --git a/y2016_bot3/actors/drivetrain_actor.cc b/y2016_bot3/actors/drivetrain_actor.cc
deleted file mode 100644
index 686c50f..0000000
--- a/y2016_bot3/actors/drivetrain_actor.cc
+++ /dev/null
@@ -1,181 +0,0 @@
-#include "y2016_bot3/actors/drivetrain_actor.h"
-
-#include <functional>
-#include <numeric>
-
-#include <Eigen/Dense>
-
-#include "aos/common/util/phased_loop.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/util/trapezoid_profile.h"
-#include "aos/common/commonmath.h"
-#include "aos/common/time.h"
-
-#include "y2016_bot3/actors/drivetrain_actor.h"
-#include "y2016_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-
-namespace y2016_bot3 {
-namespace actors {
-
-DrivetrainActor::DrivetrainActor(actors::DrivetrainActionQueueGroup *s)
- : aos::common::actions::ActorBase<actors::DrivetrainActionQueueGroup>(s),
- loop_(::y2016_bot3::control_loops::drivetrain::MakeDrivetrainLoop()) {
- loop_.set_controller_index(3);
-}
-
-bool DrivetrainActor::RunAction(const actors::DrivetrainActionParams ¶ms) {
- static const auto K = loop_.K();
-
- const double yoffset = params.y_offset;
- const double turn_offset =
- params.theta_offset * control_loops::drivetrain::kRobotRadius;
- 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(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
- 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(params.maximum_acceleration);
- profile.set_maximum_velocity(params.maximum_velocity);
- turn_profile.set_maximum_acceleration(
- params.maximum_turn_acceleration *
- control_loops::drivetrain::kRobotRadius);
- turn_profile.set_maximum_velocity(params.maximum_turn_velocity *
- control_loops::drivetrain::kRobotRadius);
-
- while (true) {
- ::aos::time::PhasedLoopXMS(5, 2500);
-
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
- if (::frc971::control_loops::drivetrain_queue.status.get()) {
- const auto &status = *::frc971::control_loops::drivetrain_queue.status;
- 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.estimated_left_position + status.estimated_right_position -
- params.left_initial_position - params.right_initial_position) /
- 2.0);
- } else {
- 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;
- } else if (status.uncapped_left_voltage < -12.0) {
- dx_left = (status.uncapped_left_voltage + 12.0) / divisor;
- } else {
- dx_left = 0;
- }
-
- if (status.uncapped_right_voltage > 12.0) {
- dx_right = (status.uncapped_right_voltage - 12.0) / divisor;
- } else if (status.uncapped_right_voltage < -12.0) {
- dx_right = (status.uncapped_right_voltage + 12.0) / divisor;
- } else {
- dx_right = 0;
- }
-
- double dx;
-
- if (dx_left == 0 && dx_right == 0) {
- dx = 0;
- } else if (dx_left != 0 && dx_right != 0 &&
- ::aos::sign(dx_left) != ::aos::sign(dx_right)) {
- // Both saturating in opposite directions. Don't do anything.
- LOG(DEBUG, "Saturating opposite ways, not adjusting\n");
- dx = 0;
- } else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
- dx = dx_left;
- } else {
- dx = dx_right;
- }
-
- if (dx != 0) {
- LOG(DEBUG, "adjusting goal by %f\n", dx);
- profile.MoveGoal(-dx);
- }
- }
- } else {
- // If we ever get here, that's bad and we should just give up
- LOG(ERROR, "no drivetrain status!\n");
- return false;
- }
-
- 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;
- }
-
- if (ShouldCancel()) return true;
-
- LOG(DEBUG, "Driving left to %f, right to %f\n",
- left_goal_state(0, 0) + params.left_initial_position,
- right_goal_state(0, 0) + params.right_initial_position);
- ::frc971::control_loops::drivetrain_queue.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .highgear(true)
- .left_goal(left_goal_state(0, 0) + params.left_initial_position)
- .right_goal(right_goal_state(0, 0) + params.right_initial_position)
- .left_velocity_goal(left_goal_state(1, 0))
- .right_velocity_goal(right_goal_state(1, 0))
- .Send();
- }
- if (ShouldCancel()) return true;
- ::frc971::control_loops::drivetrain_queue.status.FetchLatest();
-
- while (!::frc971::control_loops::drivetrain_queue.status.get()) {
- LOG(WARNING,
- "No previous drivetrain status packet, trying to fetch again\n");
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- if (ShouldCancel()) return true;
- }
-
- while (true) {
- if (ShouldCancel()) return true;
- const double kPositionThreshold = 0.05;
-
- const double left_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_left_position -
- (left_goal_state(0, 0) + params.left_initial_position));
- const double right_error =
- ::std::abs(::frc971::control_loops::drivetrain_queue.status
- ->estimated_right_position -
- (right_goal_state(0, 0) + params.right_initial_position));
- const double velocity_error = ::std::abs(
- ::frc971::control_loops::drivetrain_queue.status->robot_speed);
- if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
- velocity_error < 0.2) {
- break;
- } else {
- LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
- velocity_error);
- }
- ::frc971::control_loops::drivetrain_queue.status.FetchNextBlocking();
- }
-
- LOG(INFO, "Done moving\n");
- return true;
-}
-
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2016_bot3::actors::DrivetrainActionParams ¶ms) {
- return ::std::unique_ptr<DrivetrainAction>(
- new DrivetrainAction(&::y2016_bot3::actors::drivetrain_action, params));
-}
-
-} // namespace actors
-} // namespace y2016_bot3
diff --git a/y2016_bot3/actors/drivetrain_actor.h b/y2016_bot3/actors/drivetrain_actor.h
deleted file mode 100644
index ebf3ed5..0000000
--- a/y2016_bot3/actors/drivetrain_actor.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#ifndef Y2016_BOT3_ACTORS_DRIVETRAIN_ACTOR_H_
-#define Y2016_BOT3_ACTORS_DRIVETRAIN_ACTOR_H_
-
-#include <memory>
-
-#include "aos/common/actions/actor.h"
-#include "aos/common/actions/actions.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-
-#include "y2016_bot3/actors/drivetrain_action.q.h"
-
-namespace y2016_bot3 {
-namespace actors {
-
-class DrivetrainActor
- : public ::aos::common::actions::ActorBase<DrivetrainActionQueueGroup> {
- public:
- explicit DrivetrainActor(DrivetrainActionQueueGroup *s);
-
- bool RunAction(const actors::DrivetrainActionParams ¶ms) override;
-
- private:
- StateFeedbackLoop<4, 2, 2> loop_;
-};
-
-typedef ::aos::common::actions::TypedAction<DrivetrainActionQueueGroup>
- DrivetrainAction;
-
-// Makes a new DrivetrainActor action.
-::std::unique_ptr<DrivetrainAction> MakeDrivetrainAction(
- const ::y2016_bot3::actors::DrivetrainActionParams ¶ms);
-
-} // namespace actors
-} // namespace y2016_bot3
-
-#endif
diff --git a/y2016_bot3/actors/drivetrain_actor_main.cc b/y2016_bot3/actors/drivetrain_actor_main.cc
deleted file mode 100644
index 41a4d28..0000000
--- a/y2016_bot3/actors/drivetrain_actor_main.cc
+++ /dev/null
@@ -1,18 +0,0 @@
-#include <stdio.h>
-
-#include "aos/linux_code/init.h"
-#include "y2016_bot3/actors/drivetrain_action.q.h"
-#include "y2016_bot3/actors/drivetrain_actor.h"
-
-using ::aos::time::Time;
-
-int main(int /*argc*/, char* /*argv*/ []) {
- ::aos::Init(-1);
-
- ::y2016_bot3::actors::DrivetrainActor drivetrain(
- &::y2016_bot3::actors::drivetrain_action);
- drivetrain.Run();
-
- ::aos::Cleanup();
- return 0;
-}
diff --git a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
index cefb1cf..e6d2bc2 100644
--- a/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2016_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -36,7 +36,9 @@
kThreeStateDriveShifter,
kThreeStateDriveShifter,
true,
- 0.0};
+ 0.0,
+ 0.25,
+ 1.0};
return kDrivetrainConfig;
};
diff --git a/y2016_bot3/control_loops/drivetrain/drivetrain_base.h b/y2016_bot3/control_loops/drivetrain/drivetrain_base.h
index d6a041e..280d1c5 100644
--- a/y2016_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2016_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -8,7 +8,6 @@
static constexpr double drivetrain_max_speed = 5.0;
// The ratio from the encoder shaft to the drivetrain wheels.
-// TODO(constants): Update these.
static constexpr double kDrivetrainEncoderRatio = 1.0;
} // namespace constants
diff --git a/y2016_bot3/control_loops/intake/intake.cc b/y2016_bot3/control_loops/intake/intake.cc
index 5320cd5..d3d288b 100644
--- a/y2016_bot3/control_loops/intake/intake.cc
+++ b/y2016_bot3/control_loops/intake/intake.cc
@@ -33,28 +33,6 @@
.lpNorm<Eigen::Infinity>() < tolerance);
}
-double Intake::MoveButKeepAbove(double reference_angle, double current_angle,
- double move_distance) {
- return -MoveButKeepBelow(-reference_angle, -current_angle, -move_distance);
-}
-
-double Intake::MoveButKeepBelow(double reference_angle, double current_angle,
- double move_distance) {
- // There are 3 interesting places to move to.
- const double small_negative_move = current_angle - move_distance;
- const double small_positive_move = current_angle + move_distance;
- // And the reference angle.
-
- // Move the the highest one that is below reference_angle.
- if (small_negative_move > reference_angle) {
- return reference_angle;
- } else if (small_positive_move > reference_angle) {
- return small_negative_move;
- } else {
- return small_positive_move;
- }
-}
-
void Intake::RunIteration(const control_loops::IntakeQueue::Goal *unsafe_goal,
const control_loops::IntakeQueue::Position *position,
control_loops::IntakeQueue::Output *output,
diff --git a/y2016_bot3/control_loops/intake/intake.h b/y2016_bot3/control_loops/intake/intake.h
index fed17ab..a5b4da6 100644
--- a/y2016_bot3/control_loops/intake/intake.h
+++ b/y2016_bot3/control_loops/intake/intake.h
@@ -54,6 +54,7 @@
class IntakeTest_DisabledWhileZeroingLow_Test;
}
+// TODO(Adam): Implement this class and delete it from here.
class LimitChecker {
public:
LimitChecker(IntakeArm *intake) : intake_(intake) {}
@@ -109,15 +110,6 @@
State state() const { return state_; }
- // Returns the value to move the joint to such that it will stay below
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepBelow(double reference_angle, double current_angle,
- double move_distance);
- // Returns the value to move the joint to such that it will stay above
- // reference_angle starting at current_angle, but move at least move_distance
- static double MoveButKeepAbove(double reference_angle, double current_angle,
- double move_distance);
-
protected:
void RunIteration(const control_loops::IntakeQueue::Goal *unsafe_goal,
const control_loops::IntakeQueue::Position *position,
diff --git a/y2016_bot3/control_loops/intake/intake.q b/y2016_bot3/control_loops/intake/intake.q
index da3edbb..b6b0bdc 100644
--- a/y2016_bot3/control_loops/intake/intake.q
+++ b/y2016_bot3/control_loops/intake/intake.q
@@ -80,9 +80,6 @@
// Estimated angle and angular velocitie of the intake.
JointState intake;
-
- // Is the intake collided?
- bool is_collided;
};
message Position {
diff --git a/y2016_bot3/control_loops/intake/intake_lib_test.cc b/y2016_bot3/control_loops/intake/intake_lib_test.cc
index 43232aa..4048a19 100644
--- a/y2016_bot3/control_loops/intake/intake_lib_test.cc
+++ b/y2016_bot3/control_loops/intake/intake_lib_test.cc
@@ -443,14 +443,6 @@
EXPECT_EQ(Intake::RUNNING, intake_.state());
}
-// Tests that MoveButKeepBelow returns sane values.
-TEST_F(IntakeTest, MoveButKeepBelowTest) {
- EXPECT_EQ(1.0, Intake::MoveButKeepBelow(1.0, 10.0, 1.0));
- EXPECT_EQ(1.0, Intake::MoveButKeepBelow(1.0, 2.0, 1.0));
- EXPECT_EQ(0.0, Intake::MoveButKeepBelow(1.0, 1.0, 1.0));
- EXPECT_EQ(1.0, Intake::MoveButKeepBelow(1.0, 0.0, 1.0));
-}
-
// Tests that the integrators works.
TEST_F(IntakeTest, IntegratorTest) {
intake_plant_.InitializeIntakePosition(
diff --git a/y2016_bot3/joystick_reader.cc b/y2016_bot3/joystick_reader.cc
index a02142d..97bd089 100644
--- a/y2016_bot3/joystick_reader.cc
+++ b/y2016_bot3/joystick_reader.cc
@@ -143,11 +143,8 @@
saw_ball_when_started_intaking_ = ball_detected;
}
- if (data.IsPressed(kIntakeIn)) {
- is_intaking_ = (!ball_detected || saw_ball_when_started_intaking_);
- } else {
- is_intaking_ = false;
- }
+ is_intaking_ = data.IsPressed(kIntakeIn) &&
+ (!ball_detected || saw_ball_when_started_intaking_);
is_outtaking_ = data.IsPressed(kIntakeOut);