Port y2014 bot3 to new control system.
Change-Id: I894277089335e36ea95b52e033056b1a8fb4ca30
diff --git a/y2014_bot3/BUILD b/y2014_bot3/BUILD
new file mode 100644
index 0000000..e3de467
--- /dev/null
+++ b/y2014_bot3/BUILD
@@ -0,0 +1,21 @@
+package(default_visibility = ['//visibility:public'])
+
+cc_binary(
+ name = 'joystick_reader_y2014_bot3',
+ srcs = [
+ 'joystick_reader.cc',
+ ],
+ deps = [
+ '//aos/prime/input:joystick_input',
+ '//aos/linux_code:init',
+ '//aos/common/logging',
+ '//aos/common:time',
+ '//aos/common/util:log_interval',
+ '//aos/common/actions:action_lib',
+
+ '//frc971/queues:gyro',
+ '//y2014_bot3/autonomous:auto_queue',
+ '//y2014_bot3/control_loops/rollers:rollers_queue',
+ '//y2014_bot3/control_loops/drivetrain:drivetrain_queue',
+ ],
+)
diff --git a/y2014_bot3/actions/actions.gyp b/y2014_bot3/actions/actions.gyp
deleted file mode 100644
index a1c0130..0000000
--- a/y2014_bot3/actions/actions.gyp
+++ /dev/null
@@ -1,101 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'action_client',
- 'type': 'static_library',
- 'sources': [
- #'<(DEPTH)/frc971/actions/action_client.h',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:queues',
- ],
- },
- {
- 'target_name': 'action_queue',
- 'type': 'static_library',
- 'sources': [
- '<(DEPTH)/frc971/actions/action.q',
- ],
- 'variables': {
- 'header_path': 'frc971/actions',
- },
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'drivetrain_action_queue',
- 'type': 'static_library',
- 'sources': [
- '<(DEPTH)/frc971/actions/drivetrain_action.q',
- ],
- 'variables': {
- 'header_path': 'frc971/actions',
- },
- 'dependencies': [
- 'action_queue',
- ],
- 'export_dependent_settings': [
- 'action_queue',
- ],
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'drivetrain_action_lib',
- 'type': 'static_library',
- 'sources': [
- 'drivetrain_action.cc',
- ],
- 'dependencies': [
- 'drivetrain_action_queue',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/util/util.gyp:phased_loop',
- '<(AOS)/build/aos.gyp:logging',
- 'action_client',
- 'action',
- '<(EXTERNALS):eigen',
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:polydrivetrain_plants',
- '<(AOS)/common/util/util.gyp:trapezoid_profile',
- ],
- 'export_dependent_settings': [
- 'action',
- 'drivetrain_action_queue',
- 'action_client',
- ],
- },
- {
- 'target_name': 'action',
- 'type': 'static_library',
- 'sources': [
- #'action.h',
- ],
- 'dependencies': [
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- ],
- 'export_dependent_settings': [
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- ],
- },
- {
- 'target_name': 'drivetrain_action',
- 'type': 'executable',
- 'sources': [
- 'drivetrain_action_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'drivetrain_action_queue',
- 'drivetrain_action_lib',
- 'action',
- ],
- },
- ],
-}
diff --git a/y2014_bot3/autonomous/BUILD b/y2014_bot3/autonomous/BUILD
new file mode 100644
index 0000000..698869d
--- /dev/null
+++ b/y2014_bot3/autonomous/BUILD
@@ -0,0 +1,44 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'auto_queue',
+ srcs = [
+ 'auto.q',
+ ],
+)
+
+cc_library(
+ name = 'auto_lib',
+ srcs = [
+ 'auto.cc',
+ ],
+ hdrs = [
+ 'auto.h',
+ ],
+ deps = [
+ ':auto_queue',
+ '//aos/common/controls:control_loop',
+ '//y2014_bot3/control_loops/drivetrain:drivetrain_queue',
+ '//y2014_bot3/control_loops/drivetrain:drivetrain_lib',
+ '//y2014_bot3/control_loops/rollers:rollers_queue',
+ '//aos/common:time',
+ '//aos/common/util:phased_loop',
+ '//aos/common/util:trapezoid_profile',
+ '//aos/common/logging',
+ '//aos/common/logging:queue_logging',
+ ],
+)
+
+cc_binary(
+ name = 'auto_y2014_bot3',
+ srcs = [
+ 'auto_main.cc',
+ ],
+ deps = [
+ '//aos/linux_code:init',
+ ':auto_queue',
+ ':auto_lib',
+ ],
+)
diff --git a/y2014_bot3/autonomous/auto.cc b/y2014_bot3/autonomous/auto.cc
index ab9ffa5..bf5a3cb 100644
--- a/y2014_bot3/autonomous/auto.cc
+++ b/y2014_bot3/autonomous/auto.cc
@@ -6,19 +6,18 @@
#include "aos/common/time.h"
#include "aos/common/util/trapezoid_profile.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
-#include "bot3/actions/drivetrain_action.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
-#include "bot3/control_loops/rollers/rollers.q.h"
-#include "frc971/actions/action_client.h"
-#include "frc971/actions/drivetrain_action.q.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "y2014_bot3/autonomous/auto.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/rollers/rollers.q.h"
using ::aos::time::Time;
+using ::y2014_bot3::control_loops::drivetrain_queue;
+using ::y2014_bot3::control_loops::rollers_queue;
-namespace bot3 {
+namespace y2014_bot3 {
namespace autonomous {
namespace time = ::aos::time;
@@ -26,31 +25,18 @@
static double left_initial_position, right_initial_position;
bool ShouldExitAuto() {
- ::frc971::autonomous::autonomous.FetchLatest();
- bool ans = !::frc971::autonomous::autonomous->run_auto;
+ ::y2014_bot3::autonomous::autonomous.FetchLatest();
+ bool ans = !::y2014_bot3::autonomous::autonomous->run_auto;
if (ans) {
LOG(INFO, "Time to exit auto mode\n");
}
return ans;
}
-void StopDrivetrain() {
- LOG(INFO, "Stopping the drivetrain\n");
- control_loops::drivetrain.goal.MakeWithBuilder()
- .control_loop_driving(true)
- .left_goal(left_initial_position)
- .left_velocity_goal(0)
- .right_goal(right_initial_position)
- .right_velocity_goal(0)
- .quickturn(false)
- .Send();
-}
-
void ResetDrivetrain() {
LOG(INFO, "resetting the drivetrain\n");
- control_loops::drivetrain.goal.MakeWithBuilder()
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(false)
- .highgear(false)
.steering(0.0)
.throttle(0.0)
.left_goal(left_initial_position)
@@ -60,118 +46,48 @@
.Send();
}
-void WaitUntilDoneOrCanceled(::frc971::Action *action) {
- while (true) {
- // Poll the running bit and auto done bits.
- ::aos::time::PhasedLoop10MS(5000);
- if (!action->Running() || ShouldExitAuto()) {
- return;
- }
- }
-}
-
-::std::unique_ptr<::frc971::TypedAction
- < ::frc971::actions::DrivetrainActionQueueGroup>>
-SetDriveGoal(double distance, bool slow_acceleration,
- double maximum_velocity = 1.7, double theta = 0) {
- LOG(INFO, "Driving to %f\n", distance);
- auto drivetrain_action = actions::MakeDrivetrainAction();
- drivetrain_action->GetGoal()->left_initial_position = left_initial_position;
- drivetrain_action->GetGoal()->right_initial_position = right_initial_position;
- drivetrain_action->GetGoal()->y_offset = distance;
- drivetrain_action->GetGoal()->theta_offset = theta;
- drivetrain_action->GetGoal()->maximum_velocity = maximum_velocity;
- drivetrain_action->GetGoal()->maximum_acceleration =
- slow_acceleration ? 2.5 : 3.0;
- drivetrain_action->Start();
- left_initial_position +=
- distance - theta * control_loops::kBot3TurnWidth / 2.0;
- right_initial_position +=
- distance + theta * control_loops::kBot3TurnWidth / 2.0;
- return ::std::move(drivetrain_action);
-}
-
void InitializeEncoders() {
- control_loops::drivetrain.status.FetchLatest();
- while (!control_loops::drivetrain.status.get()) {
- LOG(WARNING,
- "No previous drivetrain position packet, trying to fetch again\n");
- control_loops::drivetrain.status.FetchNextBlocking();
- }
+ control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
- control_loops::drivetrain.status->filtered_left_position;
+ control_loops::drivetrain_queue.status->filtered_left_position;
right_initial_position =
- control_loops::drivetrain.status->filtered_right_position;
-
-}
-
-void StopRollers() {
- control_loops::rollers.goal.MakeWithBuilder()
- .intake(0)
- .low_spit(0)
- .human_player(false)
- .Send();
-}
-
-void SpitBallFront() {
- control_loops::rollers.goal.MakeWithBuilder()
- .intake(0)
- .low_spit(1)
- .human_player(false)
- .Send();
- time::SleepFor(time::Time::InSeconds(1));
- StopRollers();
-}
-
-void IntakeBallBack() {
- control_loops::rollers.goal.MakeWithBuilder()
- .intake(-1)
- .low_spit(0)
- .human_player(false)
- .Send();
- time::SleepFor(time::Time::InSeconds(1.5));
- StopRollers();
-}
-
-void ScoreBall(const double distance, const double velocity) {
- // Drive to the goal, score, and drive back.
- {
- // Drive forward.
- auto drivetrain_action = SetDriveGoal(distance,
- false, velocity);
- LOG(INFO, "Waiting until drivetrain is finished.\n");
- WaitUntilDoneOrCanceled(drivetrain_action.get());
- time::SleepFor(time::Time::InSeconds(0.5));
- if (ShouldExitAuto()) return;
- }
- {
- LOG(INFO, "Spitting ball.\n");
- SpitBallFront();
- time::SleepFor(time::Time::InSeconds(0.5));
- if (ShouldExitAuto()) return;
- }
- {
- // Drive back.
- LOG(INFO, "Driving back.\n");
- auto drivetrain_action = SetDriveGoal(-distance,
- false, velocity);
- LOG(INFO, "Waiting until drivetrain is finished.\n");
- WaitUntilDoneOrCanceled(drivetrain_action.get());
- if (ShouldExitAuto()) return;
- }
+ control_loops::drivetrain_queue.status->filtered_right_position;
}
void HandleAuto() {
- constexpr double kDriveDistance = 4.86;
- constexpr double kAutoVelocity = 2.5;
+ ::aos::time::Time start_time = ::aos::time::Time::Now();
+ LOG(INFO, "Starting auto mode at %f\n", start_time.ToSeconds());
- if (ShouldExitAuto()) return;
+ // TODO(comran): Add various options for different autos down below.
ResetDrivetrain();
InitializeEncoders();
- if (ShouldExitAuto()) return;
- ScoreBall(kDriveDistance, kAutoVelocity);
+ LOG(INFO, "Driving\n");
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .highgear(false)
+ .quickturn(false)
+ .steering(0.0)
+ .throttle(0.5)
+ .left_goal(left_initial_position)
+ .left_velocity_goal(0)
+ .right_goal(right_initial_position)
+ .right_velocity_goal(0)
+ .Send();
+ time::SleepFor(time::Time::InSeconds(2.0));
+
+ control_loops::drivetrain_queue.goal.MakeWithBuilder()
+ .control_loop_driving(false)
+ .highgear(false)
+ .quickturn(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();
}
} // namespace autonomous
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/autonomous/auto.h b/y2014_bot3/autonomous/auto.h
index 896d22c..387d2a4 100644
--- a/y2014_bot3/autonomous/auto.h
+++ b/y2014_bot3/autonomous/auto.h
@@ -1,12 +1,12 @@
-#ifndef BOT3_AUTONOMOUS_AUTO_H_
-#define BOT3_AUTONOMOUS_AUTO_H_
+#ifndef Y2014_BOT3_AUTONOMOUS_AUTO_H_
+#define Y2014_BOT3_AUTONOMOUS_AUTO_H_
-namespace bot3 {
+namespace y2014_bot3 {
namespace autonomous {
void HandleAuto();
} // namespace autonomous
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_AUTONOMOUS_AUTO_H_
+#endif // Y2014_BOT3_AUTONOMOUS_AUTO_H_
diff --git a/y2014_bot3/autonomous/auto.q b/y2014_bot3/autonomous/auto.q
new file mode 100644
index 0000000..68fcd5f
--- /dev/null
+++ b/y2014_bot3/autonomous/auto.q
@@ -0,0 +1,7 @@
+package y2014_bot3.autonomous;
+
+message AutoControl {
+ // True if auto mode should be running, false otherwise.
+ bool run_auto;
+};
+queue AutoControl autonomous;
diff --git a/y2014_bot3/autonomous/auto_main.cc b/y2014_bot3/autonomous/auto_main.cc
index eb8f865..7966d97 100644
--- a/y2014_bot3/autonomous/auto_main.cc
+++ b/y2014_bot3/autonomous/auto_main.cc
@@ -3,35 +3,35 @@
#include "aos/common/time.h"
#include "aos/linux_code/init.h"
#include "aos/common/logging/logging.h"
-#include "bot3/autonomous/auto.h"
-#include "frc971/autonomous/auto.q.h"
+#include "y2014_bot3/autonomous/auto.q.h"
+#include "y2014_bot3/autonomous/auto.h"
using ::aos::time::Time;
int main(int /*argc*/, char * /*argv*/[]) {
::aos::Init();
- LOG(INFO, "Auto main started\n");
- ::frc971::autonomous::autonomous.FetchLatest();
- while (!::frc971::autonomous::autonomous.get()) {
- ::frc971::autonomous::autonomous.FetchNextBlocking();
+ LOG(INFO, "Auto main started.\n");
+ ::y2014_bot3::autonomous::autonomous.FetchLatest();
+ while (!::y2014_bot3::autonomous::autonomous.get()) {
+ ::y2014_bot3::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
}
while (true) {
- while (!::frc971::autonomous::autonomous->run_auto) {
- ::frc971::autonomous::autonomous.FetchNextBlocking();
+ while (!::y2014_bot3::autonomous::autonomous->run_auto) {
+ ::y2014_bot3::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
}
LOG(INFO, "Starting auto mode\n");
::aos::time::Time start_time = ::aos::time::Time::Now();
- ::bot3::autonomous::HandleAuto();
+ ::y2014_bot3::autonomous::HandleAuto();
::aos::time::Time elapsed_time = ::aos::time::Time::Now() - start_time;
LOG(INFO, "Auto mode exited in %f, waiting for it to finish.\n",
elapsed_time.ToSeconds());
- while (::frc971::autonomous::autonomous->run_auto) {
- ::frc971::autonomous::autonomous.FetchNextBlocking();
+ while (::y2014_bot3::autonomous::autonomous->run_auto) {
+ ::y2014_bot3::autonomous::autonomous.FetchNextBlocking();
LOG(INFO, "Got another auto packet\n");
}
LOG(INFO, "Waiting for auto to start back up.\n");
diff --git a/y2014_bot3/autonomous/autonomous.gyp b/y2014_bot3/autonomous/autonomous.gyp
deleted file mode 100644
index 01d95d0..0000000
--- a/y2014_bot3/autonomous/autonomous.gyp
+++ /dev/null
@@ -1,50 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'auto_queue',
- 'type': 'static_library',
- 'sources': [
- '<(DEPTH)/frc971/autonomous/auto.q',
- ],
- 'variables': {
- 'header_path': 'frc971/autonomous',
- },
- 'includes': ['../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'auto_lib',
- 'type': 'static_library',
- 'sources': [
- 'auto.cc',
- ],
- 'dependencies': [
- 'auto_queue',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/bot3/control_loops/rollers/rollers.gyp:rollers_loop',
- '<(DEPTH)/bot3/actions/actions.gyp:action_client',
- '<(DEPTH)/bot3/actions/actions.gyp:drivetrain_action_lib',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/util/util.gyp:phased_loop',
- '<(AOS)/common/util/util.gyp:trapezoid_profile',
- '<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/controls/controls.gyp:control_loop',
- ],
- },
- {
- 'target_name': 'auto',
- 'type': 'executable',
- 'sources': [
- 'auto_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'auto_queue',
- 'auto_lib',
- ],
- },
- ],
-}
diff --git a/y2014_bot3/control_loops/drivetrain/BUILD b/y2014_bot3/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..c1b1433
--- /dev/null
+++ b/y2014_bot3/control_loops/drivetrain/BUILD
@@ -0,0 +1,95 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+cc_binary(
+ name = 'replay_drivetrain_y2014_bot3',
+ srcs = [
+ 'replay_drivetrain.cc',
+ ],
+ deps = [
+ ':drivetrain_queue',
+ '//aos/common/controls:replay_control_loop',
+ '//aos/linux_code:init',
+ ],
+)
+
+queue_library(
+ name = 'drivetrain_queue',
+ srcs = [
+ 'drivetrain.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ ],
+)
+
+cc_library(
+ name = 'polydrivetrain_plants',
+ srcs = [
+ 'polydrivetrain_dog_motor_plant.cc',
+ 'drivetrain_dog_motor_plant.cc',
+ 'polydrivetrain_cim_plant.cc',
+ ],
+ hdrs = [
+ 'polydrivetrain_dog_motor_plant.h',
+ 'drivetrain_dog_motor_plant.h',
+ 'polydrivetrain_cim_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'drivetrain_lib',
+ hdrs = [
+ 'drivetrain.h',
+ ],
+ srcs = [
+ 'drivetrain.cc',
+ ],
+ deps = [
+ ':polydrivetrain_plants',
+ ':drivetrain_queue',
+ '//aos/common/controls:control_loop',
+ '//aos/common/controls:polytope',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:coerce_goal',
+ '//frc971/queues:gyro',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/logging:matrix_logging',
+ '//aos/common:math',
+ '//aos/common/util:log_interval',
+ '//frc971:shifter_hall_effect',
+ ],
+)
+
+cc_test(
+ name = 'drivetrain_lib_test',
+ srcs = [
+ 'drivetrain_lib_test.cc',
+ ],
+ deps = [
+ '//aos/testing:googletest',
+ ':drivetrain_queue',
+ ':drivetrain_lib',
+ '//aos/common/controls:control_loop_test',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/queues:gyro',
+ '//aos/common:queues',
+ '//aos/common/network:team_number',
+ ],
+)
+
+cc_binary(
+ name = 'drivetrain_y2014_bot3',
+ srcs = [
+ 'drivetrain_main.cc',
+ ],
+ deps = [
+ ':drivetrain_lib',
+ ':drivetrain_queue',
+ '//aos/linux_code:init',
+ ],
+)
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.cc b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
index 3a40836..047cb71 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
@@ -1,12 +1,9 @@
-#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
-#include <math.h>
#include <stdio.h>
#include <sched.h>
-
-#include <functional>
+#include <cmath>
#include <memory>
-
#include "Eigen/Dense"
#include "aos/common/logging/logging.h"
@@ -15,21 +12,18 @@
#include "aos/common/logging/queue_logging.h"
#include "aos/common/logging/matrix_logging.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
-#include "bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "bot3/shifter_hall_effect.h"
-#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "frc971/control_loops/coerce_goal.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/queues/gyro.q.h"
+#include "frc971/shifter_hall_effect.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
using ::frc971::sensors::gyro_reading;
-using ::frc971::control_loops::DoCoerceGoal;
-using ::frc971::control_loops::CoerceGoal;
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
class DrivetrainMotorsSS {
@@ -38,35 +32,38 @@
public:
LimitedDrivetrainLoop(StateFeedbackLoop<4, 2, 2> &&loop)
: StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
- U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
- -1, 0,
- 0, 1,
- 0, -1).finished(),
- (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0,
- 12.0, 12.0).finished()) {
+ U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0)
+ .finished()) {
::aos::controls::HPolytope<0>::Init();
T << 1, -1, 1, 1;
T_inverse = T.inverse();
}
- bool output_was_capped() const {
- return output_was_capped_;
- }
+ bool output_was_capped() const { return output_was_capped_; }
private:
virtual void CapU() {
const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
+ mutable_U() =
+ U() * 12.0 / ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
+ LOG_MATRIX(DEBUG, "U is now", U());
+ // TODO(Austin): Figure out why the polytope stuff wasn't working and
+ // remove this hack.
output_was_capped_ = true;
+ return;
+
LOG_MATRIX(DEBUG, "U at start", U());
+ LOG_MATRIX(DEBUG, "R at start", R());
+ LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
Eigen::Matrix<double, 2, 2> position_K;
- position_K << K(0, 0), K(0, 2),
- K(1, 0), K(1, 2);
+ position_K << K(0, 0), K(0, 2), K(1, 0), K(1, 2);
Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << K(0, 1), K(0, 3),
- K(1, 1), K(1, 3);
+ velocity_K << K(0, 1), K(0, 3), K(1, 1), K(1, 3);
Eigen::Matrix<double, 2, 1> position_error;
position_error << error(0, 0), error(2, 0);
@@ -106,10 +103,11 @@
standard.inverse() * W;
bool is_inside_h;
- const auto adjusted_pos_error_h =
- DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+ const auto adjusted_pos_error_h = frc971::control_loops::DoCoerceGoal(
+ pos_poly, LH, wh, drive_error, &is_inside_h);
const auto adjusted_pos_error_45 =
- DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
+ frc971::control_loops::DoCoerceGoal(pos_poly, L45, w45,
+ intersection, nullptr);
if (pos_poly.IsInside(intersection)) {
adjusted_pos_error = adjusted_pos_error_h;
} else {
@@ -136,12 +134,12 @@
const ::aos::controls::HPolytope<2> U_Poly_;
Eigen::Matrix<double, 2, 2> T, T_inverse;
- bool output_was_capped_ = false;;
+ bool output_was_capped_ = false;
};
DrivetrainMotorsSS()
: loop_(new LimitedDrivetrainLoop(
- MakeDrivetrainLoop())),
+ ::y2014_bot3::control_loops::MakeDrivetrainLoop())),
filtered_offset_(0.0),
gyro_(0.0),
left_goal_(0.0),
@@ -167,9 +165,7 @@
}
void SetPosition(double left, double right, double gyro) {
// Decay the offset quickly because this gyro is great.
- const double offset =
- (right - left - gyro * kBot3TurnWidth) / 2.0;
- // TODO(brians): filtered_offset_ = offset first time around.
+ const double offset = (right - left - gyro * kDrivetrainTurnWidth) / 2.0;
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
SetRawPosition(left, right);
@@ -198,19 +194,13 @@
return (loop_->X_hat(1, 0) + loop_->X_hat(3, 0)) / 2;
}
- double GetEstimatedLeftEncoder() const {
- return loop_->X_hat(0, 0);
- }
+ double GetEstimatedLeftEncoder() const { return loop_->X_hat(0, 0); }
- double GetEstimatedRightEncoder() const {
- return loop_->X_hat(2, 0);
- }
+ double GetEstimatedRightEncoder() const { return loop_->X_hat(2, 0); }
- bool OutputWasCapped() const {
- return loop_->output_was_capped();
- }
+ bool OutputWasCapped() const { return loop_->output_was_capped(); }
- void SendMotors(Drivetrain::Output *output) const {
+ void SendMotors(DrivetrainQueue::Output *output) const {
if (output) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
@@ -234,31 +224,26 @@
class PolyDrivetrain {
public:
-
- enum Gear {
- HIGH,
- LOW,
- SHIFTING_UP,
- SHIFTING_DOWN
- };
+ enum Gear { HIGH, LOW };
// Stall Torque in N m
static constexpr double kStallTorque = 2.42;
// Stall Current in Amps
- static constexpr double kStallCurrent = 133;
+ static constexpr double kStallCurrent = 133.0;
// Free Speed in RPM. Used number from last year.
static constexpr double kFreeSpeed = 4650.0;
// Free Current in Amps
static constexpr double kFreeCurrent = 2.7;
// Moment of inertia of the drivetrain in kg m^2
// Just borrowed from last year.
- static constexpr double J = 6.4;
+ static constexpr double J = 10;
// Mass of the robot, in kg.
static constexpr double m = 68;
// Radius of the robot, in meters (from last year).
- static constexpr double rb = 0.617998644 / 2.0;
+ static constexpr double rb = 0.66675 / 2.0;
static constexpr double kWheelRadius = 0.04445;
// Resistance of the motor, divided by the number of motors.
- static constexpr double kR = (12.0 / kStallCurrent / 4 + 0.03) / (0.93 * 0.93);
+ static constexpr double kR =
+ (12.0 / kStallCurrent / 4 + 0.03) / (0.93 * 0.93);
// Motor velocity constant
static constexpr double Kv =
((kFreeSpeed / 60.0 * 2.0 * M_PI) / (12.0 - kR * kFreeCurrent));
@@ -275,7 +260,7 @@
/*[*/ 12 /*]*/,
/*[*/ 12 /*]]*/).finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(
- MakeVelocityDrivetrainLoop())),
+ ::y2014_bot3::control_loops::MakeVelocityDrivetrainLoop())),
ttrust_(1.1),
wheel_(0.0),
throttle_(0.0),
@@ -285,84 +270,44 @@
left_gear_(LOW),
right_gear_(LOW),
counter_(0) {
-
last_position_.Zero();
position_.Zero();
}
- static bool IsInGear(Gear gear) { return gear == LOW || gear == HIGH; }
- static double MotorSpeed(const constants::ShifterHallEffect &hall_effect,
- double shifter_position, double velocity, Gear gear) {
- // TODO(austin): G_high, G_low and kWheelRadius
- const double avg_hall_effect =
- (hall_effect.clear_high + hall_effect.clear_low) / 2.0;
-
- const bool use_high =
- kBot3SimpleShifting ? gear == HIGH : shifter_position > avg_hall_effect;
- if (use_high) {
- return velocity / kBot3HighGearRatio / kWheelRadius;
+ static double MotorSpeed(bool high_gear, double velocity) {
+ if (high_gear) {
+ return velocity / kDrivetrainHighGearRatio / kWheelRadius;
} else {
- return velocity / kBot3LowGearRatio / kWheelRadius;
+ return velocity / kDrivetrainLowGearRatio / kWheelRadius;
}
}
void SetGoal(double wheel, double throttle, bool quickturn, bool highgear) {
- const double kWheelNonLinearity = 0.3;
+ const double kWheelNonLinearity = 0.5;
// Apply a sin function that's scaled to make it feel better.
const double angular_range = M_PI_2 * kWheelNonLinearity;
+
+ quickturn_ = quickturn;
wheel_ = sin(angular_range * wheel) / sin(angular_range);
wheel_ = sin(angular_range * wheel_) / sin(angular_range);
- quickturn_ = quickturn;
+ if (!quickturn_) {
+ wheel_ *= 1.5;
+ }
static const double kThrottleDeadband = 0.05;
if (::std::abs(throttle) < kThrottleDeadband) {
throttle_ = 0;
} else {
throttle_ = copysign((::std::abs(throttle) - kThrottleDeadband) /
- (1.0 - kThrottleDeadband), throttle);
+ (1.0 - kThrottleDeadband),
+ throttle);
}
- // TODO(austin): Fix the upshift logic to include states.
- Gear requested_gear;
- requested_gear = highgear ? HIGH : LOW;
-
- // Can be set to HIGH and LOW instead if we want to use simple shifting.
- const Gear shift_up = kBot3SimpleShifting ? HIGH : SHIFTING_UP;
- const Gear shift_down = kBot3SimpleShifting ? LOW : SHIFTING_DOWN;
-
- if (left_gear_ != requested_gear) {
- if (IsInGear(left_gear_)) {
- if (requested_gear == HIGH) {
- left_gear_ = shift_up;
- } else {
- left_gear_ = shift_down;
- }
- } else {
- if (requested_gear == HIGH && left_gear_ == SHIFTING_DOWN) {
- left_gear_ = SHIFTING_UP;
- } else if (requested_gear == LOW && left_gear_ == SHIFTING_UP) {
- left_gear_ = SHIFTING_DOWN;
- }
- }
- }
- if (right_gear_ != requested_gear) {
- if (IsInGear(right_gear_)) {
- if (requested_gear == HIGH) {
- right_gear_ = shift_up;
- } else {
- right_gear_ = shift_down;
- }
- } else {
- if (requested_gear == HIGH && right_gear_ == SHIFTING_DOWN) {
- right_gear_ = SHIFTING_UP;
- } else if (requested_gear == LOW && right_gear_ == SHIFTING_UP) {
- right_gear_ = SHIFTING_DOWN;
- }
- }
- }
+ Gear requested_gear = highgear ? HIGH : LOW;
+ left_gear_ = right_gear_ = requested_gear;
}
- void SetPosition(const Drivetrain::Position *position) {
+ void SetPosition(const DrivetrainQueue::Position *position) {
if (position == NULL) {
++stale_count_;
} else {
@@ -371,68 +316,6 @@
position_time_delta_ = (stale_count_ + 1) * 0.01;
stale_count_ = 0;
}
-
- if (position) {
- GearLogging gear_logging;
- // Switch to the correct controller.
- const double left_middle_shifter_position =
- (kBot3LeftDriveShifter.clear_high +
- kBot3LeftDriveShifter.clear_low) / 2.0;
- const double right_middle_shifter_position =
- (kBot3RightDriveShifter.clear_high +
- kBot3RightDriveShifter.clear_low) / 2.0;
-
- if (position->left_shifter_position < left_middle_shifter_position ||
- left_gear_ == LOW) {
- if (position->right_shifter_position < right_middle_shifter_position ||
- right_gear_ == LOW) {
- gear_logging.left_loop_high = false;
- gear_logging.right_loop_high = false;
- loop_->set_controller_index(gear_logging.controller_index = 0);
- } else {
- gear_logging.left_loop_high = false;
- gear_logging.right_loop_high = true;
- loop_->set_controller_index(gear_logging.controller_index = 1);
- }
- } else {
- if (position->right_shifter_position < right_middle_shifter_position ||
- right_gear_ == LOW) {
- gear_logging.left_loop_high = true;
- gear_logging.right_loop_high = false;
- loop_->set_controller_index(gear_logging.controller_index = 2);
- } else {
- gear_logging.left_loop_high = true;
- gear_logging.right_loop_high = true;
- loop_->set_controller_index(gear_logging.controller_index = 3);
- }
- }
-
- // TODO(austin): Constants.
- if (position->left_shifter_position >
- kBot3LeftDriveShifter.clear_high &&
- left_gear_ == SHIFTING_UP) {
- left_gear_ = HIGH;
- }
- if (position->left_shifter_position <
- kBot3LeftDriveShifter.clear_low &&
- left_gear_ == SHIFTING_DOWN) {
- left_gear_ = LOW;
- }
- if (position->right_shifter_position >
- kBot3RightDriveShifter.clear_high &&
- right_gear_ == SHIFTING_UP) {
- right_gear_ = HIGH;
- }
- if (position->right_shifter_position <
- kBot3RightDriveShifter.clear_low &&
- right_gear_ == SHIFTING_DOWN) {
- right_gear_ = LOW;
- }
-
- gear_logging.left_state = left_gear_;
- gear_logging.right_state = right_gear_;
- LOG_STRUCT(DEBUG, "state", gear_logging);
- }
}
double FilterVelocity(double throttle) {
@@ -454,9 +337,10 @@
const double adjusted_ff_voltage = ::aos::Clip(
throttle * 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
- return ((adjusted_ff_voltage +
- ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) / 2.0) /
- (ttrust_ * min_K_sum + min_FF_sum));
+ return (adjusted_ff_voltage +
+ ttrust_ * min_K_sum * (loop_->X_hat(0, 0) + loop_->X_hat(1, 0)) /
+ 2.0) /
+ (ttrust_ * min_K_sum + min_FF_sum);
}
double MaxVelocity() {
@@ -473,11 +357,11 @@
::Eigen::Matrix<double, 1, 2> FF_sum = FF.colwise().sum();
int min_FF_sum_index;
const double min_FF_sum = FF_sum.minCoeff(&min_FF_sum_index);
- //const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
+ // const double min_K_sum = loop_->K().col(min_FF_sum_index).sum();
const double high_min_FF_sum = FF_high.col(0).sum();
- const double adjusted_ff_voltage = ::aos::Clip(
- 12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
+ const double adjusted_ff_voltage =
+ ::aos::Clip(12.0 * min_FF_sum / high_min_FF_sum, -12.0, 12.0);
return adjusted_ff_voltage / min_FF_sum;
}
@@ -485,47 +369,35 @@
// TODO(austin): Observer for the current velocity instead of difference
// calculations.
++counter_;
+
const double current_left_velocity =
(position_.left_encoder - last_position_.left_encoder) /
position_time_delta_;
const double current_right_velocity =
(position_.right_encoder - last_position_.right_encoder) /
position_time_delta_;
- const double left_motor_speed =
- MotorSpeed(kBot3LeftDriveShifter,
- position_.left_shifter_position,
- current_left_velocity,
- left_gear_);
- const double right_motor_speed =
- MotorSpeed(kBot3RightDriveShifter,
- position_.right_shifter_position,
- current_right_velocity,
- right_gear_);
+ const double left_motor_speed = MotorSpeed(left_gear_ == HIGH,
+ current_left_velocity);
+ const double right_motor_speed = MotorSpeed(right_gear_ == HIGH,
+ current_right_velocity);
{
CIMLogging logging;
// Reset the CIM model to the current conditions to be ready for when we
// shift.
- if (IsInGear(left_gear_)) {
- logging.left_in_gear = true;
- } else {
- logging.left_in_gear = false;
- }
+ logging.left_in_gear = true;
logging.left_motor_speed = left_motor_speed;
logging.left_velocity = current_left_velocity;
- if (IsInGear(right_gear_)) {
- logging.right_in_gear = true;
- } else {
- logging.right_in_gear = false;
- }
+
+ logging.right_in_gear = true;
logging.right_motor_speed = right_motor_speed;
logging.right_velocity = current_right_velocity;
LOG_STRUCT(DEBUG, "currently", logging);
}
- if (IsInGear(left_gear_) && IsInGear(right_gear_)) {
+ {
// FF * X = U (steady state)
const Eigen::Matrix<double, 2, 2> FF =
loop_->B().inverse() *
@@ -548,6 +420,7 @@
}
const double left_velocity = fvel - steering_velocity;
const double right_velocity = fvel + steering_velocity;
+ LOG(DEBUG, "l=%f r=%f\n", left_velocity, right_velocity);
// Integrate velocity to get the position.
// This position is used to get integral control.
@@ -565,8 +438,8 @@
U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat());
// Limit R back inside the box.
- loop_->mutable_R() =
- CoerceGoal(R_poly, equality_k, equality_w, loop_->R());
+ loop_->mutable_R() = frc971::control_loops::CoerceGoal(
+ R_poly, equality_k, equality_w, loop_->R());
}
const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
@@ -578,35 +451,18 @@
}
// TODO(austin): Model this better.
- // TODO(austin): Feed back?
+ // TODO(austin): Feedback?
loop_->mutable_X_hat() =
loop_->A() * loop_->X_hat() + loop_->B() * loop_->U();
- } else {
- // Any motor is not in gear. Speed match.
- ::Eigen::Matrix<double, 1, 1> R_left;
- ::Eigen::Matrix<double, 1, 1> R_right;
- R_left(0, 0) = left_motor_speed;
- R_right(0, 0) = right_motor_speed;
-
- const double wiggle =
- (static_cast<double>((counter_ % 20) / 10) - 0.5) * 5.0;
-
- loop_->mutable_U(0, 0) = ::aos::Clip(
- (R_left / Kv)(0, 0) + (IsInGear(left_gear_) ? 0 : wiggle),
- -12.0, 12.0);
- loop_->mutable_U(1, 0) = ::aos::Clip(
- (R_right / Kv)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
- -12.0, 12.0);
- loop_->mutable_U() *= 12.0 / position_.battery_voltage;
}
}
- void SendMotors(Drivetrain::Output *output) {
+ void SendMotors(DrivetrainQueue::Output *output) {
if (output != NULL) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
- output->left_high = left_gear_ == HIGH || left_gear_ == SHIFTING_UP;
- output->right_high = right_gear_ == HIGH || right_gear_ == SHIFTING_UP;
+ output->left_high = left_gear_ == HIGH;
+ output->right_high = right_gear_ == HIGH;
}
}
@@ -623,10 +479,11 @@
double position_time_delta_;
Gear left_gear_;
Gear right_gear_;
- Drivetrain::Position last_position_;
- Drivetrain::Position position_;
+ DrivetrainQueue::Position last_position_;
+ DrivetrainQueue::Position position_;
int counter_;
};
+
constexpr double PolyDrivetrain::kStallTorque;
constexpr double PolyDrivetrain::kStallCurrent;
constexpr double PolyDrivetrain::kFreeSpeed;
@@ -639,11 +496,10 @@
constexpr double PolyDrivetrain::Kv;
constexpr double PolyDrivetrain::Kt;
-
-void DrivetrainLoop::RunIteration(const Drivetrain::Goal *goal,
- const Drivetrain::Position *position,
- Drivetrain::Output *output,
- Drivetrain::Status * status) {
+void DrivetrainLoop::RunIteration(const DrivetrainQueue::Goal *goal,
+ const DrivetrainQueue::Position *position,
+ DrivetrainQueue::Output *output,
+ DrivetrainQueue::Status *status) {
// TODO(aschuh): These should be members of the class.
static DrivetrainMotorsSS dt_closedloop;
static PolyDrivetrain dt_openloop;
@@ -655,17 +511,22 @@
}
no_position_.Print();
- double wheel = goal->steering;
- double throttle = goal->throttle;
- bool quickturn = goal->quickturn;
- bool highgear = goal->highgear;
+ bool control_loop_driving = false;
+ if (goal) {
+ double wheel = goal->steering;
+ double throttle = goal->throttle;
+ bool quickturn = goal->quickturn;
+ bool highgear = goal->highgear;
- bool control_loop_driving = goal->control_loop_driving;
- double left_goal = goal->left_goal;
- double right_goal = goal->right_goal;
+ control_loop_driving = goal->control_loop_driving;
+ double left_goal = goal->left_goal;
+ double right_goal = goal->right_goal;
- dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
- goal->right_velocity_goal);
+ dt_closedloop.SetGoal(left_goal, goal->left_velocity_goal, right_goal,
+ goal->right_velocity_goal);
+ dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
+ }
+
if (!bad_pos) {
const double left_encoder = position->left_encoder;
const double right_encoder = position->right_encoder;
@@ -678,7 +539,6 @@
}
}
dt_openloop.SetPosition(position);
- dt_openloop.SetGoal(wheel, throttle, quickturn, highgear);
dt_openloop.Update();
if (control_loop_driving) {
@@ -699,15 +559,18 @@
if (goal) {
done = ((::std::abs(goal->left_goal -
dt_closedloop.GetEstimatedLeftEncoder()) <
- kBot3DrivetrainDoneDistance) &&
+ kDrivetrainDoneDistance) &&
(::std::abs(goal->right_goal -
dt_closedloop.GetEstimatedRightEncoder()) <
- kBot3DrivetrainDoneDistance));
+ kDrivetrainDoneDistance));
}
status->is_done = done;
status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+
+ status->filtered_left_velocity = dt_closedloop.loop().X_hat(1, 0);
+ status->filtered_right_velocity = dt_closedloop.loop().X_hat(3, 0);
status->output_was_capped = dt_closedloop.OutputWasCapped();
status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
@@ -715,4 +578,4 @@
}
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.gyp b/y2014_bot3/control_loops/drivetrain/drivetrain.gyp
deleted file mode 100644
index d17cdf2..0000000
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.gyp
+++ /dev/null
@@ -1,102 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'drivetrain_loop',
- 'type': 'static_library',
- 'sources': ['drivetrain.q'],
- 'variables': {
- 'header_path': 'bot3/control_loops/drivetrain',
- },
- 'dependencies': [
- '<(AOS)/common/controls/controls.gyp:control_loop_queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/controls/controls.gyp:control_loop_queues',
- ],
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'polydrivetrain_plants',
- 'type': 'static_library',
- 'sources': [
- 'polydrivetrain_dog_motor_plant.cc',
- 'drivetrain_dog_motor_plant.cc',
- ],
- 'dependencies': [
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- 'export_dependent_settings': [
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- ],
- },
- {
- 'target_name': 'drivetrain_constants',
- 'type': 'static_library',
- 'sources': [
- #'drivetrain_constants.h'
- ],
- 'dependencies': [
- 'polydrivetrain_plants',
- ],
- 'export_dependent_settings': [
- 'polydrivetrain_plants',
- ],
- },
- {
- 'target_name': 'drivetrain_lib',
- 'type': 'static_library',
- 'sources': [
- 'drivetrain.cc',
- 'polydrivetrain_cim_plant.cc',
- ],
- 'dependencies': [
- 'drivetrain_loop',
- 'drivetrain_constants',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- '<(AOS)/common/controls/controls.gyp:polytope',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/util/util.gyp:log_interval',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(AOS)/common/logging/logging.gyp:matrix_logging',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/controls/controls.gyp:polytope',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- 'drivetrain_loop',
- ],
- },
- {
- 'target_name': 'drivetrain_lib_test',
- 'type': 'executable',
- 'sources': [
- 'drivetrain_lib_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- 'drivetrain_loop',
- 'drivetrain_lib',
- '<(AOS)/common/controls/controls.gyp:control_loop_test',
- '<(AOS)/common/network/network.gyp:team_number',
- '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/common/common.gyp:queues',
- ],
- },
- {
- 'target_name': 'drivetrain',
- 'type': 'executable',
- 'sources': [
- 'drivetrain_main.cc',
- ],
- 'dependencies': [
- '<(AOS)/linux_code/linux_code.gyp:init',
- 'drivetrain_lib',
- 'drivetrain_loop',
- ],
- },
- ],
-}
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.h b/y2014_bot3/control_loops/drivetrain/drivetrain.h
index b2bed8c..0565820 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.h
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.h
@@ -1,5 +1,5 @@
-#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
-#define BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
#include "Eigen/Dense"
@@ -7,30 +7,46 @@
#include "aos/common/controls/control_loop.h"
#include "aos/common/controls/polytope.h"
#include "aos/common/util/log_interval.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-namespace bot3 {
+#include "frc971/shifter_hall_effect.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+
+namespace y2014_bot3 {
namespace control_loops {
+// Constants
+// TODO(comran): Get actual constants.
+constexpr double kDrivetrainTurnWidth = 0.63;
+constexpr double kDrivetrainDoneDistance = 0.02;
+constexpr double kDrivetrainEncoderRatio = 18.0 / 44.0;
+constexpr double kDrivetrainHighGearRatio =
+ kDrivetrainEncoderRatio * 18.0 / 60.0;
+constexpr double kDrivetrainLowGearRatio = kDrivetrainHighGearRatio;
+const ::frc971::constants::ShifterHallEffect kDrivetrainRightShifter{
+ 555, 657, 660, 560, 0.2, 0.7};
+const ::frc971::constants::ShifterHallEffect kDrivetrainLeftShifter{
+ 555, 660, 644, 552, 0.2, 0.7};
+// End constants
+
class DrivetrainLoop
- : public aos::controls::ControlLoop<control_loops::Drivetrain, true, false> {
+ : public aos::controls::ControlLoop<control_loops::DrivetrainQueue> {
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
- // drivetrain at bot3::control_loops::drivetrain
- explicit DrivetrainLoop(
- control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
- : aos::controls::ControlLoop<control_loops::Drivetrain, true, false>(
- my_drivetrain) {
+ // drivetrain at y2014_bot3::control_loops::drivetrain
+ explicit DrivetrainLoop(control_loops::DrivetrainQueue *my_drivetrain =
+ &control_loops::drivetrain_queue)
+ : aos::controls::ControlLoop<control_loops::DrivetrainQueue>(
+ my_drivetrain) {
::aos::controls::HPolytope<0>::Init();
}
protected:
// Executes one cycle of the control loop.
virtual void RunIteration(
- const control_loops::Drivetrain::Goal *goal,
- const control_loops::Drivetrain::Position *position,
- control_loops::Drivetrain::Output *output,
- control_loops::Drivetrain::Status *status);
+ const control_loops::DrivetrainQueue::Goal *goal,
+ const control_loops::DrivetrainQueue::Position *position,
+ control_loops::DrivetrainQueue::Output *output,
+ control_loops::DrivetrainQueue::Status *status);
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
SimpleLogInterval no_position_ = SimpleLogInterval(
@@ -38,6 +54,6 @@
};
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_H_
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.q b/y2014_bot3/control_loops/drivetrain/drivetrain.q
index f4fe3d5..c4563f9 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.q
@@ -1,15 +1,7 @@
-package bot3.control_loops;
+package y2014_bot3.control_loops;
import "aos/common/controls/control_loops.q";
-struct GearLogging {
- int8_t controller_index;
- bool left_loop_high;
- bool right_loop_high;
- int8_t left_state;
- int8_t right_state;
-};
-
struct CIMLogging {
bool left_in_gear;
bool right_in_gear;
@@ -19,7 +11,7 @@
double right_velocity;
};
-queue_group Drivetrain {
+queue_group DrivetrainQueue {
implements aos.control_loops.ControlLoop;
message Goal {
@@ -37,9 +29,6 @@
message Position {
double left_encoder;
double right_encoder;
- double left_shifter_position;
- double right_shifter_position;
- double battery_voltage;
};
message Output {
@@ -53,6 +42,8 @@
double robot_speed;
double filtered_left_position;
double filtered_right_position;
+ double filtered_left_velocity;
+ double filtered_right_velocity;
double uncapped_left_voltage;
double uncapped_right_voltage;
@@ -67,4 +58,4 @@
queue Status status;
};
-queue_group Drivetrain drivetrain;
+queue_group DrivetrainQueue drivetrain_queue;
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
index da59426..05f6a2b 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.cc
@@ -1,17 +1,17 @@
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include <vector>
#include "frc971/control_loops/state_feedback_loop.h"
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00718627304278, 0.0, 0.000575327102319, 0.0, 0.498791664199, 0.0, 0.0897184892451, 0.0, 0.000575327102319, 1.0, 0.00718627304278, 0.0, 0.0897184892451, 0.0, 0.498791664199;
+ A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000404687213064, -8.27470202964e-05, 0.0720868114296, -0.0129038552514, -8.27470202964e-05, 0.000404687213064, -0.0129038552514, 0.0720868114296;
+ B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00715806931946, 0.0, 8.27195198739e-05, 0.0, 0.491841728751, 0.0, 0.0145551608494, 0.0, 0.000706303878161, 1.0, 0.00959963582133, 0.0, 0.124279814134, 0.0, 0.921454874639;
+ A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000408743642973, -3.47646073582e-05, 0.0730863931411, -0.00611711060145, -0.000101584891631, 0.000168261415116, -0.0178746738354, 0.0330102308046;
+ B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00959963582133, 0.0, 0.000706303878161, 0.0, 0.921454874639, 0.0, 0.124279814134, 0.0, 8.27195198739e-05, 1.0, 0.00715806931946, 0.0, 0.0145551608494, 0.0, 0.491841728751;
+ A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000168261415116, -0.000101584891631, 0.0330102308046, -0.0178746738354, -3.47646073582e-05, 0.000408743642973, -0.00611711060145, 0.0730863931411;
+ B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.00959512220091, 0.0, 0.000100752776744, 0.0, 0.920202836632, 0.0, 0.0195951135, 0.0, 0.000100752776744, 1.0, 0.00959512220091, 0.0, 0.0195951135, 0.0, 0.920202836632;
+ A << 1.0, 0.00488249769672, 0.0, 2.55881088306e-05, 0.0, 0.953388055571, 0.0, 0.0100729449137, 0.0, 2.55881088306e-05, 1.0, 0.00488249769672, 0.0, 0.0100729449137, 0.0, 0.953388055571;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000170158358449, -4.23434605168e-05, 0.0335364259492, -0.00823525605573, -4.23434605168e-05, 0.000170158358449, -0.00823525605573, 0.0335364259492;
+ B << 3.0031863249e-05, -6.5399448668e-06, 0.0119133285199, -0.00257449680311, -6.5399448668e-06, 3.0031863249e-05, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 0, 0, 1, 0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,53 +73,61 @@
StateFeedbackController<4, 2, 2> MakeDrivetrainLowLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 0.798791664199, 0.0897184892451, 3.57844915691, 3.42875197165, 0.0897184892451, 0.798791664199, 3.42875197165, 3.57844915691;
+ L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
Eigen::Matrix<double, 2, 4> K;
- K << 118.526561433, 4.19900632703, 7.79020074229, 0.921948843301, 7.79020074228, 0.921948843301, 118.526561433, 4.19900632703;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowLowPlantCoefficients());
+ K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainLowHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 0.780361183952, 0.0157698223449, 2.31127571816, 1.09059511674, 0.0157698223449, 1.23293541944, 3.02157734826, 34.5051463402;
+ L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
Eigen::Matrix<double, 2, 4> K;
- K << 116.74766142, 4.04674599343, -6.38397728262, -0.174173971494, 20.8480994015, 2.49474383494, 135.799605924, 11.0272286082;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainLowHighPlantCoefficients());
+ K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighLowController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.26735275667, 0.0994717602348, 37.3766125148, 7.14618746927, 0.0994717602348, 0.745943846725, 2.20133516364, 1.9922178483;
+ L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
Eigen::Matrix<double, 2, 4> K;
- K << 135.799605924, 11.0272286082, 20.8480994015, 2.49474383494, -6.38397728263, -0.174173971495, 116.74766142, 4.04674599343;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighLowPlantCoefficients());
+ K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<4, 2, 2> MakeDrivetrainHighHighController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.22020283663, 0.0195951135, 33.6437810428, 1.9756574066, 0.0195951135, 1.22020283663, 1.9756574066, 33.6437810428;
+ L << 1.25338805557, 0.0100729449137, 74.065541605, 2.10150476024, 0.0100729449137, 1.25338805557, 2.10150476024, 74.065541605;
Eigen::Matrix<double, 2, 4> K;
- K << 136.125709167, 11.0402255404, 6.0764292977, 1.24911698877, 6.0764292977, 1.24911698877, 136.125709167, 11.0402255404;
- return StateFeedbackController<4, 2, 2>(L, K, MakeDrivetrainHighHighPlantCoefficients());
+ K << 158.010515913, 12.6294601573, 1.98944476188, 1.06520656374, 1.98944476188, 1.06520656374, 158.010515913, 12.6294601573;
+ Eigen::Matrix<double, 4, 4> A_inv;
+ A_inv << 1.0, -0.00512149525437, 0.0, 2.72716136841e-05, 0.0, 1.04900794038, 0.0, -0.0110832091253, 0.0, 2.72716136841e-05, 1.0, -0.00512149525437, 0.0, -0.0110832091253, 0.0, 1.04900794038;
+ return StateFeedbackController<4, 2, 2>(L, K, A_inv, MakeDrivetrainHighHighPlantCoefficients());
}
StateFeedbackPlant<4, 2, 2> MakeDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<4, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<4, 2, 2>(plants);
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>> plants(4);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowLowPlantCoefficients()));
+ plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainLowHighPlantCoefficients()));
+ plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighLowPlantCoefficients()));
+ plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2>>(new StateFeedbackPlantCoefficients<4, 2, 2>(MakeDrivetrainHighHighPlantCoefficients()));
+ return StateFeedbackPlant<4, 2, 2>(&plants);
}
StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop() {
- ::std::vector<StateFeedbackController<4, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController());
- return StateFeedbackLoop<4, 2, 2>(controllers);
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<4, 2, 2>>> controllers(4);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowLowController()));
+ controllers[1] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainLowHighController()));
+ controllers[2] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighLowController()));
+ controllers[3] = ::std::unique_ptr<StateFeedbackController<4, 2, 2>>(new StateFeedbackController<4, 2, 2>(MakeDrivetrainHighHighController()));
+ return StateFeedbackLoop<4, 2, 2>(&controllers);
}
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
index f5d5896..03322ed 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h
@@ -1,9 +1,9 @@
-#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<4, 2, 2> MakeDrivetrainLowLowPlantCoefficients();
@@ -27,6 +27,6 @@
StateFeedbackLoop<4, 2, 2> MakeDrivetrainLoop();
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
index 89e41c6..2c04831 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -8,15 +8,14 @@
#include "aos/common/controls/polytope.h"
#include "aos/common/controls/control_loop_test.h"
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain.h"
-#include "bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "frc971/queues/gyro.q.h"
-
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
namespace testing {
@@ -49,11 +48,11 @@
DrivetrainSimulation()
: drivetrain_plant_(
new StateFeedbackPlant<4, 2, 2>(MakeDrivetrainPlant())),
- my_drivetrain_loop_(".frc971.control_loops.drivetrain",
- 0x8a8dde77, ".frc971.control_loops.drivetrain.goal",
- ".frc971.control_loops.drivetrain.position",
- ".frc971.control_loops.drivetrain.output",
- ".frc971.control_loops.drivetrain.status") {
+ my_drivetrain_queue_(".y2014_bot3.control_loops.drivetrain",
+ 0x8a8dde77, ".y2014_bot3.control_loops.drivetrain.goal",
+ ".y2014_bot3.control_loops.drivetrain.position",
+ ".y2014_bot3.control_loops.drivetrain.output",
+ ".y2014_bot3.control_loops.drivetrain.status") {
Reinitialize();
}
@@ -76,8 +75,8 @@
const double left_encoder = GetLeftPosition();
const double right_encoder = GetRightPosition();
- ::aos::ScopedMessagePtr<control_loops::Drivetrain::Position> position =
- my_drivetrain_loop_.position.MakeMessage();
+ ::aos::ScopedMessagePtr<control_loops::DrivetrainQueue::Position> position =
+ my_drivetrain_queue_.position.MakeMessage();
position->left_encoder = left_encoder;
position->right_encoder = right_encoder;
position.Send();
@@ -87,15 +86,15 @@
void Simulate() {
last_left_position_ = drivetrain_plant_->Y(0, 0);
last_right_position_ = drivetrain_plant_->Y(1, 0);
- EXPECT_TRUE(my_drivetrain_loop_.output.FetchLatest());
- drivetrain_plant_->mutable_U() << my_drivetrain_loop_.output->left_voltage,
- my_drivetrain_loop_.output->right_voltage;
+ EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ drivetrain_plant_->mutable_U() << my_drivetrain_queue_.output->left_voltage,
+ my_drivetrain_queue_.output->right_voltage;
drivetrain_plant_->Update();
}
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> drivetrain_plant_;
private:
- Drivetrain my_drivetrain_loop_;
+ DrivetrainQueue my_drivetrain_queue_;
double last_left_position_;
double last_right_position_;
};
@@ -105,30 +104,30 @@
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointer to shared memory that
// is no longer valid.
- Drivetrain my_drivetrain_loop_;
+ DrivetrainQueue my_drivetrain_queue_;
// Create a loop and simulation plant.
DrivetrainLoop drivetrain_motor_;
DrivetrainSimulation drivetrain_motor_plant_;
- DrivetrainTest() : my_drivetrain_loop_(".frc971.control_loops.drivetrain",
+ DrivetrainTest() : my_drivetrain_queue_(".y2014_bot3.control_loops.drivetrain",
0x8a8dde77,
- ".frc971.control_loops.drivetrain.goal",
- ".frc971.control_loops.drivetrain.position",
- ".frc971.control_loops.drivetrain.output",
- ".frc971.control_loops.drivetrain.status"),
- drivetrain_motor_(&my_drivetrain_loop_),
+ ".y2014_bot3.control_loops.drivetrain.goal",
+ ".y2014_bot3.control_loops.drivetrain.position",
+ ".y2014_bot3.control_loops.drivetrain.output",
+ ".y2014_bot3.control_loops.drivetrain.status"),
+ drivetrain_motor_(&my_drivetrain_queue_),
drivetrain_motor_plant_() {
::frc971::sensors::gyro_reading.Clear();
}
void VerifyNearGoal() {
- my_drivetrain_loop_.goal.FetchLatest();
- my_drivetrain_loop_.position.FetchLatest();
- EXPECT_NEAR(my_drivetrain_loop_.goal->left_goal,
+ my_drivetrain_queue_.goal.FetchLatest();
+ my_drivetrain_queue_.position.FetchLatest();
+ EXPECT_NEAR(my_drivetrain_queue_.goal->left_goal,
drivetrain_motor_plant_.GetLeftPosition(),
1e-2);
- EXPECT_NEAR(my_drivetrain_loop_.goal->right_goal,
+ EXPECT_NEAR(my_drivetrain_queue_.goal->right_goal,
drivetrain_motor_plant_.GetRightPosition(),
1e-2);
}
@@ -140,7 +139,7 @@
// Tests that the drivetrain converges on a goal.
TEST_F(DrivetrainTest, ConvergesCorrectly) {
- my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
.left_goal(-1.0)
.right_goal(1.0).Send();
for (int i = 0; i < 200; ++i) {
@@ -154,7 +153,7 @@
// Tests that it survives disabling.
TEST_F(DrivetrainTest, SurvivesDisabling) {
- my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
+ my_drivetrain_queue_.goal.MakeWithBuilder().control_loop_driving(true)
.left_goal(-1.0)
.right_goal(1.0).Send();
for (int i = 0; i < 500; ++i) {
@@ -170,21 +169,24 @@
VerifyNearGoal();
}
-// Tests surviving bad positions.
-TEST_F(DrivetrainTest, SurvivesBadPosition) {
- my_drivetrain_loop_.goal.MakeWithBuilder().control_loop_driving(true)
- .left_goal(-1.0)
- .right_goal(1.0).Send();
- for (int i = 0; i < 500; ++i) {
- if (i > 20 && i < 200) {
- } else {
- drivetrain_motor_plant_.SendPositionMessage();
- }
+// Tests that never having a goal doesn't break.
+TEST_F(DrivetrainTest, NoGoalStart) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ }
+}
+
+// Tests that never having a goal, but having driver's station messages, doesn't
+// break.
+TEST_F(DrivetrainTest, NoGoalWithRobotState) {
+ for (int i = 0; i < 20; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
SimulateTimestep(true);
}
- VerifyNearGoal();
}
::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
@@ -291,4 +293,4 @@
} // namespace testing
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
index f06acc0..588bc89 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -1,10 +1,10 @@
-#include "bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
#include "aos/linux_code/init.h"
int main() {
::aos::Init();
- bot3::control_loops::DrivetrainLoop drivetrain;
+ y2014_bot3::control_loops::DrivetrainLoop drivetrain;
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
index 1287483..75bae13 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.cc
@@ -1,17 +1,17 @@
-#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include <vector>
#include "frc971/control_loops/state_feedback_loop.h"
-namespace frc971 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients() {
Eigen::Matrix<double, 1, 1> A;
- A << 0.614537580221;
+ A << 0.783924473544;
Eigen::Matrix<double, 1, 1> B;
- B << 15.9657598852;
+ B << 8.94979586973;
Eigen::Matrix<double, 1, 1> C;
C << 1;
Eigen::Matrix<double, 1, 1> D;
@@ -25,23 +25,25 @@
StateFeedbackController<1, 1, 1> MakeCIMController() {
Eigen::Matrix<double, 1, 1> L;
- L << 0.604537580221;
+ L << 0.773924473544;
Eigen::Matrix<double, 1, 1> K;
- K << 0.0378646293422;
- return StateFeedbackController<1, 1, 1>(L, K, MakeCIMPlantCoefficients());
+ K << 0.086473980503;
+ Eigen::Matrix<double, 1, 1> A_inv;
+ A_inv << 1.2756330919;
+ return StateFeedbackController<1, 1, 1>(L, K, A_inv, MakeCIMPlantCoefficients());
}
StateFeedbackPlant<1, 1, 1> MakeCIMPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<1, 1, 1> *> plants(1);
- plants[0] = new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients());
- return StateFeedbackPlant<1, 1, 1>(plants);
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>> plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<1, 1, 1>>(new StateFeedbackPlantCoefficients<1, 1, 1>(MakeCIMPlantCoefficients()));
+ return StateFeedbackPlant<1, 1, 1>(&plants);
}
StateFeedbackLoop<1, 1, 1> MakeCIMLoop() {
- ::std::vector<StateFeedbackController<1, 1, 1> *> controllers(1);
- controllers[0] = new StateFeedbackController<1, 1, 1>(MakeCIMController());
- return StateFeedbackLoop<1, 1, 1>(controllers);
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<1, 1, 1>>> controllers(1);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<1, 1, 1>>(new StateFeedbackController<1, 1, 1>(MakeCIMController()));
+ return StateFeedbackLoop<1, 1, 1>(&controllers);
}
} // namespace control_loops
-} // namespace frc971
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
index 12b2c59..0def071 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_cim_plant.h
@@ -1,9 +1,9 @@
-#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
-#define FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
-namespace frc971 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<1, 1, 1> MakeCIMPlantCoefficients();
@@ -15,6 +15,6 @@
StateFeedbackLoop<1, 1, 1> MakeCIMLoop();
} // namespace control_loops
-} // namespace frc971
+} // namespace y2014_bot3
-#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_CIM_PLANT_H_
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
index 540741c..8fc4049 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.cc
@@ -1,17 +1,17 @@
-#include "bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
#include <vector>
#include "frc971/control_loops/state_feedback_loop.h"
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.498791664199, 0.0897184892451, 0.0897184892451, 0.498791664199;
+ A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0720868114296, -0.0129038552514, -0.0129038552514, 0.0720868114296;
+ B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -25,9 +25,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.491841728751, 0.0145551608494, 0.124279814134, 0.921454874639;
+ A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0730863931411, -0.00611711060145, -0.0178746738354, 0.0330102308046;
+ B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -41,9 +41,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighLowPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.921454874639, 0.124279814134, 0.0145551608494, 0.491841728751;
+ A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0330102308046, -0.0178746738354, -0.00611711060145, 0.0730863931411;
+ B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -57,9 +57,9 @@
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainHighHighPlantCoefficients() {
Eigen::Matrix<double, 2, 2> A;
- A << 0.920202836632, 0.0195951135, 0.0195951135, 0.920202836632;
+ A << 0.953388055571, 0.0100729449137, 0.0100729449137, 0.953388055571;
Eigen::Matrix<double, 2, 2> B;
- B << 0.0335364259492, -0.00823525605573, -0.00823525605573, 0.0335364259492;
+ B << 0.0119133285199, -0.00257449680311, -0.00257449680311, 0.0119133285199;
Eigen::Matrix<double, 2, 2> C;
C << 1.0, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 2> D;
@@ -73,53 +73,61 @@
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.478791664199, 0.0897184892451, 0.0897184892451, 0.478791664199;
+ L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
Eigen::Matrix<double, 2, 2> K;
- K << -1.22029287467, 1.0261517351, 1.0261517351, -1.22029287467;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowLowPlantCoefficients());
+ K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainLowHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.450790078471, 0.12008012565, 0.12008012565, 0.922506524919;
+ L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
Eigen::Matrix<double, 2, 2> K;
- K << -1.22005303899, 1.06234079364, 3.10424257883, 10.3132835345;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainLowHighPlantCoefficients());
+ K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainLowHighPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighLowController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.918551885829, 0.0172871978996, 0.0172871978996, 0.454744717561;
+ L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
Eigen::Matrix<double, 2, 2> K;
- K << 10.3132835345, 3.10424257883, 1.06234079364, -1.22005303899;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighLowPlantCoefficients());
+ K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighLowPlantCoefficients());
}
StateFeedbackController<2, 2, 2> MakeVelocityDrivetrainHighHighController() {
Eigen::Matrix<double, 2, 2> L;
- L << 0.900202836632, 0.0195951135, 0.0195951135, 0.900202836632;
+ L << 0.933388055571, 0.0100729449137, 0.0100729449137, 0.933388055571;
Eigen::Matrix<double, 2, 2> K;
- K << 10.3132878272, 3.11684016931, 3.11684016931, 10.3132878272;
- return StateFeedbackController<2, 2, 2>(L, K, MakeVelocityDrivetrainHighHighPlantCoefficients());
+ K << 31.3080614945, 7.61126069778, 7.61126069778, 31.3080614945;
+ Eigen::Matrix<double, 2, 2> A_inv;
+ A_inv << 1.04900794038, -0.0110832091253, -0.0110832091253, 1.04900794038;
+ return StateFeedbackController<2, 2, 2>(L, K, A_inv, MakeVelocityDrivetrainHighHighPlantCoefficients());
}
StateFeedbackPlant<2, 2, 2> MakeVelocityDrivetrainPlant() {
- ::std::vector<StateFeedbackPlantCoefficients<2, 2, 2> *> plants(4);
- plants[0] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients());
- plants[1] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients());
- plants[2] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients());
- plants[3] = new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients());
- return StateFeedbackPlant<2, 2, 2>(plants);
+ ::std::vector< ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>> plants(4);
+ plants[0] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowLowPlantCoefficients()));
+ plants[1] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainLowHighPlantCoefficients()));
+ plants[2] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighLowPlantCoefficients()));
+ plants[3] = ::std::unique_ptr<StateFeedbackPlantCoefficients<2, 2, 2>>(new StateFeedbackPlantCoefficients<2, 2, 2>(MakeVelocityDrivetrainHighHighPlantCoefficients()));
+ return StateFeedbackPlant<2, 2, 2>(&plants);
}
StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop() {
- ::std::vector<StateFeedbackController<2, 2, 2> *> controllers(4);
- controllers[0] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController());
- controllers[1] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController());
- controllers[2] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController());
- controllers[3] = new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController());
- return StateFeedbackLoop<2, 2, 2>(controllers);
+ ::std::vector< ::std::unique_ptr<StateFeedbackController<2, 2, 2>>> controllers(4);
+ controllers[0] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowLowController()));
+ controllers[1] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainLowHighController()));
+ controllers[2] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighLowController()));
+ controllers[3] = ::std::unique_ptr<StateFeedbackController<2, 2, 2>>(new StateFeedbackController<2, 2, 2>(MakeVelocityDrivetrainHighHighController()));
+ return StateFeedbackLoop<2, 2, 2>(&controllers);
}
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
index 191d1aa..4442d35 100644
--- a/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
+++ b/y2014_bot3/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h
@@ -1,9 +1,9 @@
-#ifndef BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
-#define BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#define Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
#include "frc971/control_loops/state_feedback_loop.h"
-namespace bot3 {
+namespace y2014_bot3 {
namespace control_loops {
StateFeedbackPlantCoefficients<2, 2, 2> MakeVelocityDrivetrainLowLowPlantCoefficients();
@@ -27,6 +27,6 @@
StateFeedbackLoop<2, 2, 2> MakeVelocityDrivetrainLoop();
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_DRIVETRAIN_POLYDRIVETRAIN_DOG_MOTOR_PLANT_H_
diff --git a/y2014_bot3/control_loops/drivetrain/replay_drivetrain.cc b/y2014_bot3/control_loops/drivetrain/replay_drivetrain.cc
new file mode 100644
index 0000000..4f83c9e
--- /dev/null
+++ b/y2014_bot3/control_loops/drivetrain/replay_drivetrain.cc
@@ -0,0 +1,24 @@
+#include "aos/common/controls/replay_control_loop.h"
+#include "aos/linux_code/init.h"
+
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+
+// Reads one or more log files and sends out all the queue messages (in the
+// correct order and at the correct time) to feed a "live" drivetrain process.
+
+int main(int argc, char **argv) {
+ if (argc <= 1) {
+ fprintf(stderr, "Need at least one file to replay!\n");
+ return EXIT_FAILURE;
+ }
+
+ ::aos::InitNRT();
+
+ ::aos::controls::ControlLoopReplayer<::y2014_bot3::control_loops::DrivetrainQueue>
+ replayer(&::y2014_bot3::control_loops::drivetrain_queue, "drivetrain");
+ for (int i = 1; i < argc; ++i) {
+ replayer.ProcessFile(argv[i]);
+ }
+
+ ::aos::Cleanup();
+}
diff --git a/y2014_bot3/control_loops/python/drivetrain.py b/y2014_bot3/control_loops/python/drivetrain.py
index bc0df04..c0c4595 100755
--- a/y2014_bot3/control_loops/python/drivetrain.py
+++ b/y2014_bot3/control_loops/python/drivetrain.py
@@ -56,30 +56,33 @@
# Stall Torque in N m
self.stall_torque = 2.42
# Stall Current in Amps
- self.stall_current = 133
+ self.stall_current = 133.0
# Free Speed in RPM. Used number from last year.
self.free_speed = 4650.0
# Free Current in Amps
self.free_current = 2.7
# Moment of inertia of the drivetrain in kg m^2
# Just borrowed from last year.
- self.J = 4.5
+ self.J = 10
# Mass of the robot, in kg.
- self.m = 60
+ self.m = 68
# Radius of the robot, in meters (from last year).
- self.rb = 0.7112 / 2.0
+ self.rb = 0.9603 / 2.0
# Radius of the wheels, in meters.
- self.r = .04445
+ self.r = 0.0508
# Resistance of the motor, divided by the number of motors.
- self.R = 12.0 / self.stall_current / 4
+ self.R = 12.0 / self.stall_current / 2
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
# Gear ratios
- self.G_low = 14.0 / 60.0 * 17.0 / 50.0
- self.G_high = 30.0 / 44.0 * 17.0 / 50.0
+ self.G_const = 18.0 / 44.0 * 18.0 / 60.0
+
+ self.G_low = self.G_const
+ self.G_high = self.G_const
+
if left_low:
self.Gl = self.G_low
else:
@@ -88,8 +91,9 @@
self.Gr = self.G_low
else:
self.Gr = self.G_high
+
# Control loop time step
- self.dt = 0.01
+ self.dt = 0.005
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
@@ -223,13 +227,13 @@
drivetrain_high_low = Drivetrain(name="DrivetrainHighLow", left_low=False, right_low=True)
drivetrain_high_high = Drivetrain(name="DrivetrainHighHigh", left_low=False, right_low=False)
- if len(argv) != 3:
+ if len(argv) != 5:
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
"Drivetrain", [drivetrain_low_low, drivetrain_low_high,
drivetrain_high_low, drivetrain_high_high],
- namespaces = ["bot3", "control_loops"])
+ namespaces=['y2014_bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
else:
diff --git a/y2014_bot3/control_loops/python/polydrivetrain.py b/y2014_bot3/control_loops/python/polydrivetrain.py
index 49d390b..2ffbed6 100755
--- a/y2014_bot3/control_loops/python/polydrivetrain.py
+++ b/y2014_bot3/control_loops/python/polydrivetrain.py
@@ -102,7 +102,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ self.dt = 0.005
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
[self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
@@ -168,7 +168,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
@@ -177,7 +177,7 @@
# ttrust is the comprimise between having full throttle negative inertia,
# and having no throttle negative inertia. A value of 0 is full throttle
# inertia. A value of 1 is no throttle negative inertia.
- self.ttrust = 1.0
+ self.ttrust = 1.1
self.left_gear = VelocityDrivetrain.LOW
self.right_gear = VelocityDrivetrain.LOW
@@ -302,26 +302,30 @@
# This is the same as sending the most torque down to the floor at the
# wheel.
- self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
- current_gear=self.left_gear,
- gear_name="left")
- self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
- current_gear=self.right_gear,
- gear_name="right")
- if self.IsInGear(self.left_gear):
- self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
+ self.left_gear = self.right_gear = True
+ if False:
+ self.left_gear = self.ComputeGear(self.X[0, 0], should_print=True,
+ current_gear=self.left_gear,
+ gear_name="left")
+ self.right_gear = self.ComputeGear(self.X[1, 0], should_print=True,
+ current_gear=self.right_gear,
+ gear_name="right")
+ if self.IsInGear(self.left_gear):
+ self.left_cim.X[0, 0] = self.MotorRPM(self.left_shifter_position, self.X[0, 0])
- if self.IsInGear(self.right_gear):
- self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
+ if self.IsInGear(self.right_gear):
+ self.right_cim.X[0, 0] = self.MotorRPM(self.right_shifter_position, self.X[0, 0])
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ steering *= 2.3
+ if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
# Filter the throttle to provide a nicer response.
fvel = self.FilterVelocity(throttle)
# Constant radius means that angualar_velocity / linear_velocity = constant.
# Compute the left and right velocities.
- left_velocity = fvel - steering * numpy.abs(fvel)
- right_velocity = fvel + steering * numpy.abs(fvel)
+ steering_velocity = numpy.abs(fvel) * steering
+ left_velocity = fvel - steering_velocity
+ right_velocity = fvel + steering_velocity
# Write this constraint in the form of K * R = w
# angular velocity / linear velocity = constant
@@ -379,7 +383,7 @@
# TODO(austin): Model the robot as not accelerating when you shift...
# This hack only works when you shift at the same time.
- if self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
+ if True or self.IsInGear(self.left_gear) and self.IsInGear(self.right_gear):
self.X = self.CurrentDrivetrain().A * self.X + self.CurrentDrivetrain().B * self.U
self.left_gear, self.left_shifter_position = self.SimShifter(
@@ -394,7 +398,7 @@
def main(argv):
vdrivetrain = VelocityDrivetrain()
- if len(argv) != 5:
+ if len(argv) != 7:
print "Expected .h file name and .cc file name"
else:
dog_loop_writer = control_loop.ControlLoopWriter(
@@ -402,7 +406,7 @@
vdrivetrain.drivetrain_low_high,
vdrivetrain.drivetrain_high_low,
vdrivetrain.drivetrain_high_high],
- namespaces = ["bot3", "control_loops"])
+ namespaces=['y2014_bot3', 'control_loops'])
if argv[1][-3:] == '.cc':
dog_loop_writer.Write(argv[2], argv[1])
@@ -410,12 +414,13 @@
dog_loop_writer.Write(argv[1], argv[2])
cim_writer = control_loop.ControlLoopWriter(
- "CIM", [drivetrain.CIM()])
+ "CIM", [drivetrain.CIM()],
+ namespaces=['y2014_bot3', 'control_loops'])
- if argv[3][-3:] == '.cc':
- cim_writer.Write(argv[4], argv[3])
+ if argv[5][-3:] == '.cc':
+ cim_writer.Write(argv[6], argv[5])
else:
- cim_writer.Write(argv[3], argv[4])
+ cim_writer.Write(argv[5], argv[6])
return
vl_plot = []
@@ -442,13 +447,13 @@
else:
print "Right is low"
- for t in numpy.arange(0, 4.0, vdrivetrain.dt):
- if t < 1.0:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ for t in numpy.arange(0, 1.7, vdrivetrain.dt):
+ if t < 0.5:
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
elif t < 1.2:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ vdrivetrain.Update(throttle=0.5, steering=1.0)
else:
- vdrivetrain.Update(throttle=1.00, steering=0.0)
+ vdrivetrain.Update(throttle=0.00, steering=1.0)
t_plot.append(t)
vl_plot.append(vdrivetrain.X[0, 0])
vr_plot.append(vdrivetrain.X[1, 0])
@@ -475,10 +480,10 @@
cim_velocity_plot.append(cim.X[0, 0])
cim_voltage_plot.append(U[0, 0] * 10)
cim_time.append(t)
- #pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
- #pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
- #pylab.legend()
- #pylab.show()
+ pylab.plot(cim_time, cim_velocity_plot, label='cim spinup')
+ pylab.plot(cim_time, cim_voltage_plot, label='cim voltage')
+ pylab.legend()
+ pylab.show()
# TODO(austin):
# Shifting compensation.
diff --git a/y2014_bot3/control_loops/rollers/BUILD b/y2014_bot3/control_loops/rollers/BUILD
new file mode 100644
index 0000000..e3526ac
--- /dev/null
+++ b/y2014_bot3/control_loops/rollers/BUILD
@@ -0,0 +1,40 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'rollers_queue',
+ srcs = [
+ 'rollers.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ '//frc971/control_loops:queues',
+ ],
+)
+
+cc_library(
+ name = 'rollers_lib',
+ hdrs = [
+ 'rollers.h',
+ ],
+ srcs = [
+ 'rollers.cc',
+ ],
+ deps = [
+ ':rollers_queue',
+ '//aos/common/logging',
+ '//aos/common/controls:control_loop',
+ ],
+)
+
+cc_binary(
+ name = 'rollers',
+ srcs = [
+ 'rollers_main.cc',
+ ],
+ deps = [
+ ':rollers_lib',
+ '//aos/linux_code:init',
+ ],
+)
diff --git a/y2014_bot3/control_loops/rollers/rollers.cc b/y2014_bot3/control_loops/rollers/rollers.cc
index 8777549..ef895b3 100644
--- a/y2014_bot3/control_loops/rollers/rollers.cc
+++ b/y2014_bot3/control_loops/rollers/rollers.cc
@@ -1,40 +1,45 @@
-#include "bot3/control_loops/rollers/rollers.h"
-#include "bot3/control_loops/rollers/rollers.q.h"
+#include "y2014_bot3/control_loops/rollers/rollers.h"
-namespace bot3 {
+#include "aos/common/logging/logging.h"
+
+namespace y2014_bot3 {
namespace control_loops {
-void RollersLoop::RunIteration(const Rollers::Goal *goal,
- const Rollers::Position * /*position*/,
- Rollers::Output *output,
- Rollers::Status * /*status*/) {
- constexpr double kBot3IntakeForwardVoltage = 12.0;
- constexpr double kBot3IntakeBackwardVoltage = -12.0;
- constexpr double kBot3LowGoalForwardVoltage = 6.0;
- constexpr double kBot3LowGoalBackwardVoltage = -6.0;
+Rollers::Rollers(control_loops::RollersQueue *rollers)
+ : aos::controls::ControlLoop<control_loops::RollersQueue>(rollers) {}
+
+void Rollers::RunIteration(
+ const control_loops::RollersQueue::Goal *goal,
+ const control_loops::RollersQueue::Position * /*position*/,
+ control_loops::RollersQueue::Output *output,
+ control_loops::RollersQueue::Status * /*status*/) {
+ constexpr double k2014Bot3IntakeForwardVoltage = 12.0;
+ constexpr double k2014Bot3IntakeBackwardVoltage = -12.0;
+ constexpr double k2014Bot3LowGoalForwardVoltage = 6.0;
+ constexpr double k2014Bot3LowGoalBackwardVoltage = -6.0;
+
+ if (!output || !goal) {
+ return;
+ }
const int intake = goal->intake;
const int low_spit = goal->low_spit;
const bool human_player = goal->human_player;
- if (!output) {
- return;
- }
-
output->Zero();
switch (low_spit) {
case 1:
// Spit towards front
- output->low_goal_voltage = kBot3LowGoalBackwardVoltage;
- output->front_intake_voltage = kBot3IntakeBackwardVoltage;
- output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+ output->low_goal_voltage = k2014Bot3LowGoalBackwardVoltage;
+ output->front_intake_voltage = k2014Bot3IntakeBackwardVoltage;
+ output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
break;
case -1:
// Spit towards back
- output->low_goal_voltage = kBot3LowGoalForwardVoltage;
- output->back_intake_voltage = -kBot3IntakeBackwardVoltage;
- output->front_intake_voltage = kBot3IntakeForwardVoltage;
+ output->low_goal_voltage = k2014Bot3LowGoalForwardVoltage;
+ output->back_intake_voltage = -k2014Bot3IntakeBackwardVoltage;
+ output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
break;
default:
// Stationary
@@ -46,14 +51,14 @@
// Front intake.
output->front_extended = true;
output->back_extended = false;
- output->front_intake_voltage = kBot3IntakeForwardVoltage;
+ output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
output->back_intake_voltage = 0.0;
break;
case -1:
// Back intake.
output->back_extended = true;
output->front_extended = false;
- output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+ output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
output->front_intake_voltage = 0.0;
break;
default:
@@ -65,10 +70,10 @@
// Intake for human player.
output->front_extended = false;
output->back_extended = false;
- output->front_intake_voltage = kBot3IntakeForwardVoltage;
- output->back_intake_voltage = -kBot3IntakeForwardVoltage;
+ output->front_intake_voltage = k2014Bot3IntakeForwardVoltage;
+ output->back_intake_voltage = -k2014Bot3IntakeForwardVoltage;
}
}
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
diff --git a/y2014_bot3/control_loops/rollers/rollers.gyp b/y2014_bot3/control_loops/rollers/rollers.gyp
deleted file mode 100644
index e865ed8..0000000
--- a/y2014_bot3/control_loops/rollers/rollers.gyp
+++ /dev/null
@@ -1,45 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'rollers_loop',
- 'type': 'static_library',
- 'sources': ['rollers.q'],
- 'variables': {
- 'header_path': 'bot3/control_loops/rollers',
- },
- 'dependencies': [
- '<(AOS)/common/controls/controls.gyp:control_loop_queues',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/controls/controls.gyp:control_loop_queues',
- ],
- 'includes': ['../../../aos/build/queues.gypi'],
- },
- {
- 'target_name': 'rollers_lib',
- 'type': 'static_library',
- 'sources': [
- 'rollers.cc',
- ],
- 'dependencies': [
- 'rollers_loop',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- ],
- 'export_dependent_settings': [
- 'rollers_loop',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- ],
- },
- {
- 'target_name': 'rollers',
- 'type': 'executable',
- 'sources': [
- 'rollers_main.cc',
- ],
- 'dependencies': [
- 'rollers_lib',
- '<(AOS)/linux_code/linux_code.gyp:init'
- ],
- },
- ],
-}
diff --git a/y2014_bot3/control_loops/rollers/rollers.h b/y2014_bot3/control_loops/rollers/rollers.h
index f67b372..f0cae13 100644
--- a/y2014_bot3/control_loops/rollers/rollers.h
+++ b/y2014_bot3/control_loops/rollers/rollers.h
@@ -1,33 +1,29 @@
-#ifndef BOT3_CONTROL_LOOPS_ROLLERS_H_
-#define BOT3_CONTROL_LOOPS_ROLLERS_H_
+#ifndef Y2014_BOT3_CONTROL_LOOPS_ROLLERS_H_
+#define Y2014_BOT3_CONTROL_LOOPS_ROLLERS_H_
#include "aos/common/controls/control_loop.h"
-#include "bot3/control_loops/rollers/rollers.q.h"
-namespace bot3 {
+#include "y2014_bot3/control_loops/rollers/rollers.q.h"
+
+namespace y2014_bot3 {
namespace control_loops {
-class RollersLoop
- : public aos::controls::ControlLoop<control_loops::Rollers,
- false, false, true> {
+class Rollers : public aos::controls::ControlLoop<control_loops::RollersQueue> {
public:
// Constructs a control loops which can take a rollers or defaults to the
- // rollers at ::bot3::control_loops::rollers.
- explicit RollersLoop(
- control_loops::Rollers *my_rollers = &control_loops::rollers)
- : aos::controls::ControlLoop<control_loops::Rollers, false, false, true>(
- my_rollers) {}
+ // rollers at ::2014_bot3::control_loops::rollers.
+ explicit Rollers(control_loops::RollersQueue *rollers_queue =
+ &control_loops::rollers_queue);
protected:
// Executes one cycle of the control loop.
- virtual void RunIteration(
- const control_loops::Rollers::Goal *goal,
- const control_loops::Rollers::Position *position,
- control_loops::Rollers::Output *output,
- control_loops::Rollers::Status *status);
+ void RunIteration(const control_loops::RollersQueue::Goal *goal,
+ const control_loops::RollersQueue::Position *position,
+ control_loops::RollersQueue::Output *output,
+ control_loops::RollersQueue::Status *status) override;
};
} // namespace control_loops
-} // namespace bot3
+} // namespace y2014_bot3
-#endif // BOT3_CONTROL_LOOPS_ROLLERS_H_
+#endif // Y2014_BOT3_CONTROL_LOOPS_ROLLERS_H_
diff --git a/y2014_bot3/control_loops/rollers/rollers.q b/y2014_bot3/control_loops/rollers/rollers.q
index 5abb488..f218a41 100644
--- a/y2014_bot3/control_loops/rollers/rollers.q
+++ b/y2014_bot3/control_loops/rollers/rollers.q
@@ -1,8 +1,9 @@
-package bot3.control_loops;
+package y2014_bot3.control_loops;
import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
-queue_group Rollers {
+queue_group RollersQueue {
implements aos.control_loops.ControlLoop;
message Goal {
@@ -37,4 +38,4 @@
queue Status status;
};
-queue_group Rollers rollers;
+queue_group RollersQueue rollers_queue;
diff --git a/y2014_bot3/control_loops/rollers/rollers_main.cc b/y2014_bot3/control_loops/rollers/rollers_main.cc
index 75d2cc6..4700a6e 100644
--- a/y2014_bot3/control_loops/rollers/rollers_main.cc
+++ b/y2014_bot3/control_loops/rollers/rollers_main.cc
@@ -1,10 +1,10 @@
-#include "bot3/control_loops/rollers/rollers.h"
+#include "y2014_bot3/control_loops/rollers/rollers.h"
#include "aos/linux_code/init.h"
int main() {
::aos::Init();
- bot3::control_loops::RollersLoop rollers;
+ ::y2014_bot3::control_loops::Rollers rollers;
rollers.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014_bot3/control_loops/update_drivetrain.sh b/y2014_bot3/control_loops/update_drivetrain.sh
index 91ff838..b5757d3 100755
--- a/y2014_bot3/control_loops/update_drivetrain.sh
+++ b/y2014_bot3/control_loops/update_drivetrain.sh
@@ -1,9 +1,12 @@
#!/bin/bash
#
-# Updates the drivetrain controllers for the third robot.
+# Updates the drivetrain controllers.
cd $(dirname $0)
-PYTHONPATH=$PYTHONPATH:../../frc971/control_loops/python \
+export PYTHONPATH=../../frc971/control_loops/python
+
./python/drivetrain.py drivetrain/drivetrain_dog_motor_plant.h \
- drivetrain/drivetrain_dog_motor_plant.cc
+ drivetrain/drivetrain_dog_motor_plant.cc \
+ drivetrain/drivetrain_clutch_motor_plant.h \
+ drivetrain/drivetrain_clutch_motor_plant.cc
diff --git a/y2014_bot3/control_loops/update_polydrivetrain.sh b/y2014_bot3/control_loops/update_polydrivetrain.sh
index d8e2d7a..b1bb2b4 100755
--- a/y2014_bot3/control_loops/update_polydrivetrain.sh
+++ b/y2014_bot3/control_loops/update_polydrivetrain.sh
@@ -1,11 +1,14 @@
#!/bin/bash
#
-# Updates the polydrivetrain controllers and CIM models for the third robot.
+# Updates the polydrivetrain controllers and CIM models.
cd $(dirname $0)
-PYTHONPATH=$PYTHONPATH:../../frc971/control_loops/python \
+export PYTHONPATH=../../frc971/control_loops/python
+
./python/polydrivetrain.py drivetrain/polydrivetrain_dog_motor_plant.h \
drivetrain/polydrivetrain_dog_motor_plant.cc \
+ drivetrain/polydrivetrain_clutch_motor_plant.h \
+ drivetrain/polydrivetrain_clutch_motor_plant.cc \
drivetrain/polydrivetrain_cim_plant.h \
drivetrain/polydrivetrain_cim_plant.cc
diff --git a/y2014_bot3/input/input.gyp b/y2014_bot3/input/input.gyp
deleted file mode 100644
index aec28b7..0000000
--- a/y2014_bot3/input/input.gyp
+++ /dev/null
@@ -1,42 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'joystick_reader',
- 'type': 'executable',
- 'sources': [
- 'joystick_reader.cc',
- ],
- 'dependencies': [
- '<(AOS)/prime/input/input.gyp:joystick_input',
- '<(AOS)/linux_code/linux_code.gyp:init',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/util/util.gyp:log_interval',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
- '<(DEPTH)/bot3/control_loops/rollers/rollers.gyp:rollers_loop',
- ],
- },
- {
- 'target_name': 'sensor_receiver',
- 'type': 'executable',
- 'sources': [
- 'sensor_receiver.cc',
- ],
- 'dependencies': [
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_constants',
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/bot3/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/linux_code/linux_code.gyp:init',
- '<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/util/util.gyp:wrapping_counter',
- '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:sensor_reader',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(AOS)/common/controls/controls.gyp:output_check',
- ],
- },
- ],
-}
diff --git a/y2014_bot3/input/joystick_reader.cc b/y2014_bot3/input/joystick_reader.cc
deleted file mode 100644
index b8ac058..0000000
--- a/y2014_bot3/input/joystick_reader.cc
+++ /dev/null
@@ -1,173 +0,0 @@
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-#include <math.h>
-
-#include "aos/linux_code/init.h"
-#include "aos/prime/input/joystick_input.h"
-#include "aos/common/input/driver_station_data.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/util/log_interval.h"
-#include "aos/common/time.h"
-
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
-#include "bot3/control_loops/rollers/rollers.q.h"
-#include "frc971/autonomous/auto.q.h"
-#include "frc971/queues/other_sensors.q.h"
-
-using ::bot3::control_loops::drivetrain;
-using ::frc971::sensors::gyro_reading;
-
-using ::aos::input::driver_station::ButtonLocation;
-using ::aos::input::driver_station::JoystickAxis;
-using ::aos::input::driver_station::ControlBit;
-
-namespace bot3 {
-namespace input {
-namespace joysticks {
-
-const ButtonLocation kDriveControlLoopEnable1(1, 7),
- kDriveControlLoopEnable2(1, 11);
-const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(4, 2);
-const ButtonLocation kShiftHigh(4, 1), kShiftLow(4, 3);
-const ButtonLocation kQuickTurn(1, 5);
-
-const ButtonLocation kFrontRollersIn(3, 5);
-const ButtonLocation kBackRollersIn(3, 3);
-const ButtonLocation kFrontRollersOut(3, 12);
-const ButtonLocation kBackRollersOut(3, 8);
-const ButtonLocation kHumanPlayer(3, 11);
-
-class Reader : public ::aos::input::JoystickInput {
- public:
- Reader()
- : is_high_gear_(false) {}
-
- virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
- if (data.GetControlBit(ControlBit::kAutonomous)) {
- if (data.PosEdge(ControlBit::kEnabled)){
- LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(true)
- .Send();
- } else if (data.NegEdge(ControlBit::kEnabled)) {
- LOG(INFO, "Stopping auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder()
- .run_auto(false)
- .Send();
- } else if (!data.GetControlBit(ControlBit::kEnabled)) {
- auto goal = drivetrain.goal.MakeMessage();
- goal->Zero();
- goal->control_loop_driving = false;
- goal->left_goal = goal->right_goal = 0;
- goal->left_velocity_goal = goal->right_velocity_goal = 0;
- if (!goal.Send()) {
- LOG(WARNING, "sending 0 drivetrain goal failed\n");
- }
- }
- } else {
- HandleTeleop(data);
- }
- }
-
- 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))) {
- 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.position.FetchLatest() && gyro_reading.FetchLatest()) {
- distance = (drivetrain.position->left_encoder +
- drivetrain.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.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)
- .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) {
- HandleDrivetrain(data);
-
- // Rollers.
- auto rollers_goal = control_loops::rollers.goal.MakeMessage();
- if (data.IsPressed(kFrontRollersIn)) {
- rollers_goal->intake = 1;
- } else if (data.IsPressed(kFrontRollersOut)) {
- rollers_goal->low_spit = 1;
- } else if (data.IsPressed(kBackRollersIn)) {
- rollers_goal->intake = -1;
- } else if (data.IsPressed(kBackRollersOut)) {
- rollers_goal->low_spit = -1;
- } else if (data.IsPressed(kHumanPlayer)) {
- rollers_goal->human_player = true;
- }
- if (!rollers_goal.Send()) {
- LOG(WARNING, "Sending rollers values failed.\n");
- }
- }
-
- private:
- bool is_high_gear_;
-};
-
-} // namespace joysticks
-} // namespace input
-} // namespace bot3
-
-int main() {
- ::aos::Init();
- ::bot3::input::joysticks::Reader reader;
- reader.Run();
- ::aos::Cleanup();
-}
diff --git a/y2014_bot3/input/sensor_receiver.cc b/y2014_bot3/input/sensor_receiver.cc
deleted file mode 100644
index 683d5d1..0000000
--- a/y2014_bot3/input/sensor_receiver.cc
+++ /dev/null
@@ -1,148 +0,0 @@
-#include <inttypes.h>
-
-#include "aos/linux_code/init.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/util/wrapping_counter.h"
-#include "aos/common/time.h"
-#include "aos/common/logging/queue_logging.h"
-#include "aos/common/controls/output_check.q.h"
-
-#include "bbb/sensor_reader.h"
-
-#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/control_loops/drivetrain/drivetrain_constants.h"
-#include "bot3/queues/to_log.q.h"
-#include "bot3/shifter_hall_effect.h"
-#include "frc971/queues/other_sensors.q.h"
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-using ::bot3::control_loops::drivetrain;
-using ::frc971::sensors::gyro_reading;
-using ::aos::util::WrappingCounter;
-
-namespace bot3 {
-namespace {
-
-double drivetrain_translate(int32_t in) {
- return static_cast<double>(in)
- / (256.0 /*cpr*/ * 4.0 /*quad*/)
- * (18.0 / 50.0 /*output stage*/) * (64.0 / 24.0 /*encoder gears*/)
- // * constants::GetValues().drivetrain_encoder_ratio
- * (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * -1.0;
-}
-
-static const double kVcc = 5.15;
-
-// Translates values from the ADC into voltage.
-double adc_translate(uint16_t in) {
- if (false) {
- // This is the simple theoretical math.
- static const uint16_t kMaximumValue = 0x3FF;
- static const double kR1 = 5, kR2 = 6.65;
- const double raw =
- (kVcc * static_cast<double>(in) / static_cast<double>(kMaximumValue));
- return (raw * (kR1 + kR2) - (kVcc / 2) * kR2) / kR1;
- } else {
- // This is from a linear regression calculated with some actual data points.
- static const double kM = 0.012133, kB = -3.6813;
- return static_cast<double>(in) * kM + kB;
- }
-}
-
-double battery_translate(uint16_t in_high, uint16_t in_low) {
- const double high = adc_translate(in_high), low = adc_translate(in_low);
- static const double kDividerBig = 5.55, kDividerSmall = 2.66;
- return (high - low) * (kDividerBig + kDividerSmall) / kDividerSmall +
- kDividerBig / kDividerSmall * kVcc;
-}
-
-double gyro_translate(int64_t in) {
- return in / 16.0 / 1000.0 / (180.0 / M_PI);
-}
-
-double hall_translate(const constants::ShifterHallEffect & k,
- uint16_t in_value) {
- double out = (in_value - static_cast<double>(k.low)) /
- static_cast<double>(k.high - k.low);
- return out;
-}
-
-void PacketReceived(const ::bbb::DataStruct *data,
- const ::aos::time::Time &cape_timestamp) {
- ::aos::time::TimeFreezer time_freezer;
-
- ::frc971::logging_structs::CapeReading reading_to_log(
- cape_timestamp, static_cast<uint16_t>(sizeof(*data)),
- data->main.low_left_drive_hall, data->main.high_left_drive_hall,
- data->main.low_right_drive_hall, data->main.high_right_drive_hall);
- LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
- bool bad_gyro;
- // TODO(brians): Switch to LogInterval for these things.
- if (data->uninitialized_gyro) {
- LOG(DEBUG, "uninitialized gyro\n");
- bad_gyro = true;
- } else if (data->zeroing_gyro) {
- LOG(DEBUG, "zeroing gyro\n");
- bad_gyro = true;
- } else if (data->bad_gyro) {
- LOG(ERROR, "bad gyro\n");
- bad_gyro = true;
- } else if (data->old_gyro_reading) {
- LOG(WARNING, "old/bad gyro reading\n");
- bad_gyro = true;
- } else {
- bad_gyro = false;
- }
-
- if (!bad_gyro) {
- gyro_reading.MakeWithBuilder()
- .angle(gyro_translate(data->gyro_angle))
- .Send();
- }
-
- if (data->analog_errors != 0) {
- LOG(WARNING, "%" PRIu8 " analog errors\n", data->analog_errors);
- }
-
- if (data->main.output_check_pulse_length != 0) {
- auto message = ::aos::controls::output_check_received.MakeMessage();
- // TODO(brians): Fix this math to match what the cRIO actually does.
- // It's close but not quite right.
- message->pulse_length =
- static_cast<double>(data->main.output_check_pulse_length) / 10000.0;
- if (message->pulse_length > 2.7) {
- LOG(WARNING, "insane PWM pulse length %fms\n", message->pulse_length);
- } else {
- // TODO(danielp): Send the actual pulse length if we ever add that cable.
- message->pwm_value = 1.0;
- LOG_STRUCT(DEBUG, "received", *message);
- message.Send();
- }
- }
-
- drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->main.right_drive))
- .left_encoder(-drivetrain_translate(data->main.left_drive))
- .left_shifter_position(hall_translate(control_loops::kBot3LeftDriveShifter,
- data->main.low_left_drive_hall))
- .right_shifter_position(hall_translate(control_loops::kBot3RightDriveShifter,
- data->main.low_right_drive_hall))
- .battery_voltage(battery_translate(data->main.battery_voltage_high,
- data->main.battery_voltage_low))
- .Send();
-}
-
-} // namespace
-} // namespace bot3
-
-int main() {
- ::aos::Init(::bbb::SensorReader::kRelativePriority);
- ::bbb::SensorReader reader("comp");
- while (true) {
- ::bot3::PacketReceived(reader.ReadPacket(), reader.GetCapeTimestamp());
- }
- ::aos::Cleanup();
-}
diff --git a/y2014_bot3/joystick_reader.cc b/y2014_bot3/joystick_reader.cc
new file mode 100644
index 0000000..3f5f5a3
--- /dev/null
+++ b/y2014_bot3/joystick_reader.cc
@@ -0,0 +1,136 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/linux_code/init.h"
+#include "aos/prime/input/joystick_input.h"
+#include "aos/common/input/driver_station_data.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/time.h"
+
+#include "frc971/queues/gyro.q.h"
+#include "y2014_bot3/autonomous/auto.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014_bot3/control_loops/rollers/rollers.q.h"
+
+using ::y2014_bot3::control_loops::drivetrain_queue;
+using ::y2014_bot3::control_loops::rollers_queue;
+using ::frc971::sensors::gyro_reading;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::POVLocation;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::ControlBit;
+
+namespace y2014_bot3 {
+namespace input {
+namespace joysticks {
+
+// Joystick & button addresses.
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kQuickTurn(1, 5);
+
+const ButtonLocation kFrontRollersIn(3, 8);
+const ButtonLocation kBackRollersIn(3, 7);
+const ButtonLocation kFrontRollersOut(3, 6);
+const ButtonLocation kBackRollersOut(4, 12);
+const ButtonLocation kHumanPlayer(4, 11);
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ Reader() : is_high_gear_(false) {}
+
+ virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
+ bool last_auto_running = auto_running_;
+ auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
+ data.GetControlBit(ControlBit::kEnabled);
+ if (auto_running_ != last_auto_running) {
+ if (auto_running_) {
+ StartAuto();
+ } else {
+ StopAuto();
+ }
+ }
+
+ if (!data.GetControlBit(ControlBit::kAutonomous)) {
+ HandleDrivetrain(data);
+ HandleTeleop(data);
+ }
+ }
+
+ void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
+ const double wheel = -data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+
+ if (!drivetrain_queue.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(false)
+ .highgear(is_high_gear_)
+ .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) {
+ // Rollers.
+ auto rollers_goal = control_loops::rollers_queue.goal.MakeMessage();
+ rollers_goal->Zero();
+ if (data.IsPressed(kFrontRollersIn)) {
+ rollers_goal->intake = 1;
+ } else if (data.IsPressed(kFrontRollersOut)) {
+ rollers_goal->low_spit = 1;
+ } else if (data.IsPressed(kBackRollersIn)) {
+ rollers_goal->intake = -1;
+ } else if (data.IsPressed(kBackRollersOut)) {
+ rollers_goal->low_spit = -1;
+ } else if (data.IsPressed(kHumanPlayer)) {
+ rollers_goal->human_player = true;
+ }
+ if (!rollers_goal.Send()) {
+ LOG(WARNING, "Sending rollers values failed.\n");
+ }
+ }
+
+ private:
+ void StartAuto() {
+ LOG(INFO, "Starting auto mode.\n");
+ ::y2014_bot3::autonomous::autonomous.MakeWithBuilder().run_auto(true).Send();
+ }
+
+ void StopAuto() {
+ LOG(INFO, "Stopping auto mode\n");
+ ::y2014_bot3::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
+ }
+
+ bool auto_running_ = false;
+
+ bool is_high_gear_;
+
+ ::aos::util::SimpleLogInterval no_drivetrain_status_ =
+ ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
+ "no drivetrain status");
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2014_bot3
+
+int main() {
+ ::aos::Init();
+ ::y2014_bot3::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}
diff --git a/y2014_bot3/output/output.gyp b/y2014_bot3/output/output.gyp
deleted file mode 100644
index 5e4ff3a..0000000
--- a/y2014_bot3/output/output.gyp
+++ /dev/null
@@ -1,24 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'motor_writer',
- 'type': 'executable',
- 'sources': [
- 'motor_writer.cc'
- ],
- 'dependencies': [
- '<(AOS)/prime/output/output.gyp:motor_output',
- '<(AOS)/linux_code/linux_code.gyp:init',
- '<(AOS)/build/aos.gyp:logging',
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/bot3/control_loops/rollers/rollers.gyp:rollers_loop',
- '<(AOS)/common/controls/controls.gyp:control_loop',
- '<(DEPTH)/bot3/queues/queues.gyp:queues',
- '<(AOS)/common/util/util.gyp:log_interval',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/logging/logging.gyp:queue_logging',
- '<(AOS)/common/controls/controls.gyp:output_check',
- ],
- },
- ],
-}
diff --git a/y2014_bot3/prime/BUILD b/y2014_bot3/prime/BUILD
new file mode 100644
index 0000000..2f70798
--- /dev/null
+++ b/y2014_bot3/prime/BUILD
@@ -0,0 +1,18 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/prime/downloader/downloader', 'aos_downloader')
+
+aos_downloader(
+ name = 'download',
+ start_srcs = [
+ '//aos:prime_start_binaries',
+ '//y2014_bot3/control_loops/drivetrain:drivetrain_y2014_bot3',
+ '//y2014_bot3/control_loops/rollers:rollers',
+ '//y2014_bot3/autonomous:auto_y2014_bot3',
+ '//y2014_bot3:joystick_reader_y2014_bot3',
+ '//y2014_bot3/wpilib:wpilib_interface_y2014_bot3',
+ ],
+ srcs = [
+ '//aos:prime_binaries',
+ ],
+)
diff --git a/y2014_bot3/prime/build.sh b/y2014_bot3/prime/build.sh
index 6ae1c81..b730cff 100755
--- a/y2014_bot3/prime/build.sh
+++ b/y2014_bot3/prime/build.sh
@@ -2,4 +2,4 @@
cd $(dirname $0)
-exec ../../aos/build/build.py $0 bot3_prime prime.gyp "$@"
+exec ../../aos/build/build.py $0 prime y2014_bot3 prime.gyp "$@"
diff --git a/y2014_bot3/prime/prime.gyp b/y2014_bot3/prime/prime.gyp
deleted file mode 100644
index 986f9fa..0000000
--- a/y2014_bot3/prime/prime.gyp
+++ /dev/null
@@ -1,52 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'All',
- 'type': 'none',
- 'dependencies': [
- '<(AOS)/build/aos_all.gyp:Prime',
- '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:all_tests',
-
- '../control_loops/drivetrain/drivetrain.gyp:drivetrain',
- '../control_loops/drivetrain/drivetrain.gyp:drivetrain_lib_test',
- '../control_loops/rollers/rollers.gyp:rollers',
- '../autonomous/autonomous.gyp:auto',
- '../actions/actions.gyp:drivetrain_action',
- '../input/input.gyp:joystick_reader',
- '../output/output.gyp:motor_writer',
- '../input/input.gyp:sensor_receiver',
- '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:uart_reader_main',
- '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:test_sensor_receiver',
- '<(DEPTH)/bbb_cape/src/flasher/flasher.gyp:stm32_flasher',
- '<(AOS)/prime/input/input.gyp:joystick_proxy',
- ],
- 'variables': {
- 'cape_src': '<(DEPTH)/bbb_cape/src/cape',
- 'cape_hex': '<(cape_src)/.obj/main_comp.hex',
- },
- 'actions': [
- {
- 'action_name': 'make_cape',
- 'inputs': [
- '<!@(find <(cape_src) -name ".*" -prune -o -type f -print)',
- '<(cape_src)/Makefile',
- ],
- 'outputs': [
- '<(cape_hex)',
- ],
- 'action': ['make', '-C', '<(cape_src)'],
- 'message': 'Building cape code',
- },
- ],
- 'copies': [
- {
- 'destination': '<(rsync_dir)',
- 'files': [
- '<(cape_hex)',
- 'start_list.txt',
- ],
- },
- ],
- },
- ],
-}
diff --git a/y2014_bot3/prime/start_list.txt b/y2014_bot3/prime/start_list.txt
index 6b08ba0..b1620e1 100644
--- a/y2014_bot3/prime/start_list.txt
+++ b/y2014_bot3/prime/start_list.txt
@@ -1,10 +1,6 @@
+joystick_reader_y2014_bot3
+wpilib_interface_y2014_bot3
binary_log_writer
-motor_writer
-joystick_reader
-drivetrain
-drivetrain_action
-auto
-sensor_receiver
-joystick_proxy
+drivetrain_y2014_bot3
rollers
-auto
+auto_y2014_bot3
diff --git a/y2014_bot3/queues/queues.gyp b/y2014_bot3/queues/queues.gyp
deleted file mode 100644
index a7f03c2..0000000
--- a/y2014_bot3/queues/queues.gyp
+++ /dev/null
@@ -1,18 +0,0 @@
-{
- 'variables': {
- 'queue_files': [
- 'to_log.q',
- ]
- },
- 'targets': [
- {
- 'target_name': 'queues',
- 'type': 'static_library',
- 'sources': ['<@(queue_files)'],
- 'variables': {
- 'header_path': 'bot3/queues',
- },
- 'includes': ['../../aos/build/queues.gypi'],
- },
- ],
-}
diff --git a/y2014_bot3/wpilib/BUILD b/y2014_bot3/wpilib/BUILD
new file mode 100644
index 0000000..228003d
--- /dev/null
+++ b/y2014_bot3/wpilib/BUILD
@@ -0,0 +1,31 @@
+package(default_visibility = ['//visibility:public'])
+
+cc_binary(
+ name = 'wpilib_interface_y2014_bot3',
+ srcs = [
+ 'wpilib_interface.cc',
+ ],
+ deps = [
+ '//aos/linux_code:init',
+ '//aos/common:stl_mutex',
+ '//aos/common/logging',
+ '//aos/externals:wpilib',
+ '//y2014_bot3/control_loops/drivetrain:drivetrain_queue',
+ '//aos/common/controls:control_loop',
+ '//aos/common/util:log_interval',
+ '//aos/common:time',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/messages:robot_state',
+ '//aos/common/util:phased_loop',
+ '//aos/common/util:wrapping_counter',
+ '//frc971/wpilib:joystick_sender',
+ '//frc971/wpilib:loop_output_handler',
+ '//frc971/wpilib:buffered_pcm',
+ '//frc971/wpilib:gyro_sender',
+ '//frc971/control_loops:queues',
+ '//frc971/wpilib:logging_queue',
+ '//y2014_bot3/autonomous:auto_queue',
+ '//y2014_bot3/control_loops/drivetrain:drivetrain_lib',
+ '//y2014_bot3/control_loops/rollers:rollers_lib',
+ ],
+)
diff --git a/y2014_bot3/wpilib/wpilib_interface.cc b/y2014_bot3/wpilib/wpilib_interface.cc
new file mode 100644
index 0000000..089b1ab
--- /dev/null
+++ b/y2014_bot3/wpilib/wpilib_interface.cc
@@ -0,0 +1,419 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <inttypes.h>
+
+#include <thread>
+#include <mutex>
+#include <functional>
+
+#include "Encoder.h"
+#include "Talon.h"
+#include "DriverStation.h"
+#include "AnalogInput.h"
+#include "Compressor.h"
+#include "Relay.h"
+#include "RobotBase.h"
+#include "dma.h"
+#include "ControllerPower.h"
+#include "DigitalInput.h"
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/util/wrapping_counter.h"
+#include "aos/common/stl_mutex.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/messages/robot_state.q.h"
+
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
+#include "y2014_bot3/control_loops/rollers/rollers.q.h"
+#include "y2014_bot3/autonomous/auto.q.h"
+
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/gyro_sender.h"
+#include "frc971/wpilib/logging.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/rollers/rollers.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::aos::util::SimpleLogInterval;
+using ::y2014_bot3::control_loops::drivetrain_queue;
+using ::y2014_bot3::control_loops::rollers_queue;
+using ::frc971::wpilib::BufferedPcm;
+using ::frc971::wpilib::BufferedSolenoid;
+using ::frc971::wpilib::LoopOutputHandler;
+using ::frc971::wpilib::JoystickSender;
+using ::frc971::wpilib::GyroSender;
+
+namespace frc971 {
+namespace wpilib {
+
+double drivetrain_translate(int32_t in) {
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) *
+ ::y2014_bot3::control_loops::kDrivetrainEncoderRatio *
+ (4 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
+// Reads in our inputs. (sensors, voltages, etc.)
+class SensorReader {
+ public:
+ SensorReader() {}
+
+ void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
+ drivetrain_left_encoder_ = ::std::move(encoder);
+ }
+
+ void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
+ drivetrain_right_encoder_ = ::std::move(encoder);
+ }
+
+ void operator()() {
+ ::aos::SetCurrentThreadName("SensorReader");
+ LOG(INFO, "In sensor reader thread\n");
+
+ my_pid_ = getpid();
+ ds_ = DriverStation::GetInstance();
+
+ LOG(INFO, "Things are now started\n");
+
+ ::aos::SetCurrentThreadRealtimePriority(kPriority);
+ while (run_) {
+ ::aos::time::PhasedLoopXMS(5, 4000);
+ RunIteration();
+ }
+ }
+
+ void RunIteration() {
+ // General
+ {
+ auto new_state = ::aos::robot_state.MakeMessage();
+
+ new_state->reader_pid = my_pid_;
+ new_state->outputs_enabled = ds_->IsSysActive();
+ new_state->browned_out = ds_->IsSysBrownedOut();
+
+ new_state->is_3v3_active = ControllerPower::GetEnabled3V3();
+ new_state->is_5v_active = ControllerPower::GetEnabled5V();
+ new_state->voltage_3v3 = ControllerPower::GetVoltage3V3();
+ new_state->voltage_5v = ControllerPower::GetVoltage5V();
+
+ new_state->voltage_roborio_in = ControllerPower::GetInputVoltage();
+ new_state->voltage_battery = ds_->GetBatteryVoltage();
+
+ LOG_STRUCT(DEBUG, "robot_state", *new_state);
+
+ new_state.Send();
+ }
+
+ // Drivetrain
+ {
+ auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ drivetrain_message->right_encoder =
+ drivetrain_translate(drivetrain_right_encoder_->GetRaw());
+ drivetrain_message->left_encoder =
+ -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
+
+ drivetrain_message.Send();
+ }
+
+ // Rollers
+ {
+ auto rollers_message = rollers_queue.position.MakeMessage();
+ rollers_message.Send();
+ }
+ }
+
+ void Quit() { run_ = false; }
+
+ private:
+ static const int kPriority = 30;
+ static const int kInterruptPriority = 55;
+
+ int32_t my_pid_;
+ DriverStation *ds_;
+
+ ::std::unique_ptr<Encoder> drivetrain_left_encoder_;
+ ::std::unique_ptr<Encoder> drivetrain_right_encoder_;
+
+ ::std::atomic<bool> run_{true};
+};
+
+// Writes out our pneumatic outputs.
+class SolenoidWriter {
+ public:
+ SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
+ : pcm_(pcm),
+ drivetrain_(".y2014_bot3.control_loops.drivetrain_queue.output"),
+ rollers_(".y2014_bot3.control_loops.rollers_queue.output") {}
+
+ void set_pressure_switch(::std::unique_ptr<DigitalInput> pressure_switch) {
+ pressure_switch_ = ::std::move(pressure_switch);
+ }
+
+ void set_compressor_relay(::std::unique_ptr<Relay> compressor_relay) {
+ compressor_relay_ = ::std::move(compressor_relay);
+ }
+
+ void set_drivetrain_left(::std::unique_ptr<BufferedSolenoid> s) {
+ drivetrain_left_ = ::std::move(s);
+ }
+
+ void set_drivetrain_right(::std::unique_ptr<BufferedSolenoid> s) {
+ drivetrain_right_ = ::std::move(s);
+ }
+
+ void set_rollers_front(::std::unique_ptr<BufferedSolenoid> s) {
+ rollers_front_ = ::std::move(s);
+ }
+
+ void set_rollers_back(::std::unique_ptr<BufferedSolenoid> s) {
+ rollers_back_ = ::std::move(s);
+ }
+
+ void operator()() {
+ ::aos::SetCurrentThreadName("Solenoids");
+ ::aos::SetCurrentThreadRealtimePriority(30);
+
+ while (run_) {
+ ::aos::time::PhasedLoopXMS(20, 1000);
+ // Drivetrain
+ {
+ drivetrain_.FetchLatest();
+ if (drivetrain_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
+ drivetrain_left_->Set(drivetrain_->left_high);
+ drivetrain_right_->Set(drivetrain_->right_high);
+ }
+ }
+
+ // Intake
+ {
+ rollers_.FetchLatest();
+ if (rollers_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *rollers_);
+ rollers_front_->Set(rollers_->front_extended);
+ rollers_back_->Set(rollers_->back_extended);
+ }
+ }
+
+ // Compressor
+ ::aos::joystick_state.FetchLatest();
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+ {
+ // Refill if pneumatic pressure goes too low.
+ const bool compressor_on = !pressure_switch_->Get();
+ to_log.compressor_on = compressor_on;
+ if (compressor_on) {
+ compressor_relay_->Set(Relay::kForward);
+ } else {
+ compressor_relay_->Set(Relay::kOff);
+ }
+ }
+
+ pcm_->Flush();
+ to_log.read_solenoids = pcm_->GetAll();
+ LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ }
+ }
+ }
+
+ void Quit() { run_ = false; }
+
+ private:
+ const ::std::unique_ptr<BufferedPcm> &pcm_;
+
+ ::std::unique_ptr<BufferedSolenoid> drivetrain_left_, drivetrain_right_;
+ ::std::unique_ptr<BufferedSolenoid> rollers_front_, rollers_back_;
+
+ ::std::unique_ptr<DigitalInput> pressure_switch_;
+ ::std::unique_ptr<Relay> compressor_relay_;
+
+ ::aos::Queue<::y2014_bot3::control_loops::DrivetrainQueue::Output>
+ drivetrain_;
+ ::aos::Queue<::y2014_bot3::control_loops::RollersQueue::Output> rollers_;
+
+ ::std::atomic<bool> run_{true};
+};
+
+// Writes out drivetrain voltages.
+class DrivetrainWriter : public LoopOutputHandler {
+ public:
+ void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
+ left_drivetrain_talon_ = ::std::move(t);
+ }
+
+ void set_right_drivetrain_talon(::std::unique_ptr<Talon> t) {
+ right_drivetrain_talon_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::y2014_bot3::control_loops::drivetrain_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::y2014_bot3::control_loops::drivetrain_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ left_drivetrain_talon_->Set(-queue->left_voltage / 12.0);
+ right_drivetrain_talon_->Set(queue->right_voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "drivetrain output too old\n");
+ left_drivetrain_talon_->Disable();
+ right_drivetrain_talon_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> left_drivetrain_talon_;
+ ::std::unique_ptr<Talon> right_drivetrain_talon_;
+};
+
+// Writes out rollers voltages.
+class RollersWriter : public LoopOutputHandler {
+ public:
+ void set_rollers_front_intake_talon(::std::unique_ptr<Talon> t_left, ::std::unique_ptr<Talon> t_right) {
+ rollers_front_left_intake_talon_ = ::std::move(t_left);
+ rollers_front_right_intake_talon_ = ::std::move(t_right);
+ }
+
+ void set_rollers_back_intake_talon(::std::unique_ptr<Talon> t_left, ::std::unique_ptr<Talon> t_right) {
+ rollers_back_left_intake_talon_ = ::std::move(t_left);
+ rollers_back_right_intake_talon_ = ::std::move(t_right);
+ }
+
+ void set_rollers_low_goal_talon(::std::unique_ptr<Talon> t) {
+ rollers_low_goal_talon_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::y2014_bot3::control_loops::rollers_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::y2014_bot3::control_loops::rollers_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ rollers_front_left_intake_talon_->Set(queue->front_intake_voltage / 12.0);
+ rollers_front_right_intake_talon_->Set(-(queue->front_intake_voltage / 12.0));
+ rollers_back_left_intake_talon_->Set(queue->back_intake_voltage / 12.0);
+ rollers_back_right_intake_talon_->Set(-(queue->back_intake_voltage / 12.0));
+ rollers_low_goal_talon_->Set(queue->low_goal_voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "Intake output too old\n");
+ rollers_front_left_intake_talon_->Disable();
+ rollers_front_right_intake_talon_->Disable();
+ rollers_back_left_intake_talon_->Disable();
+ rollers_back_right_intake_talon_->Disable();
+ rollers_low_goal_talon_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> rollers_front_left_intake_talon_,
+ rollers_back_left_intake_talon_, rollers_front_right_intake_talon_,
+ rollers_back_right_intake_talon_, rollers_low_goal_talon_;
+};
+
+// TODO(brian): Replace this with ::std::make_unique once all our toolchains
+// have support.
+template <class T, class... U>
+std::unique_ptr<T> make_unique(U &&... u) {
+ return std::unique_ptr<T>(new T(std::forward<U>(u)...));
+}
+
+class WPILibRobot : public RobotBase {
+ public:
+ ::std::unique_ptr<Encoder> make_encoder(int index) {
+ return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
+ Encoder::k4X);
+ }
+ virtual void StartCompetition() {
+ ::aos::InitNRT();
+ ::aos::SetCurrentThreadName("StartCompetition");
+
+ JoystickSender joystick_sender;
+ ::std::thread joystick_thread(::std::ref(joystick_sender));
+
+ //TODO(comran): IO ports are placeholders at the moment, so match them to
+ // the robot before turning on.
+
+ // Sensors
+ SensorReader reader;
+ LOG(INFO, "Creating the reader\n");
+ reader.set_drivetrain_left_encoder(make_encoder(4));
+ reader.set_drivetrain_right_encoder(make_encoder(5));
+
+ ::std::thread reader_thread(::std::ref(reader));
+ GyroSender gyro_sender;
+ ::std::thread gyro_thread(::std::ref(gyro_sender));
+
+ // Outputs
+ DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_drivetrain_talon(
+ ::std::unique_ptr<Talon>(new Talon(2)));
+ drivetrain_writer.set_right_drivetrain_talon(
+ ::std::unique_ptr<Talon>(new Talon(5)));
+ ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+
+ RollersWriter rollers_writer;
+ rollers_writer.set_rollers_front_intake_talon(
+ ::std::unique_ptr<Talon>(new Talon(3)), ::std::unique_ptr<Talon>(new Talon(7)));
+ rollers_writer.set_rollers_back_intake_talon(
+ ::std::unique_ptr<Talon>(new Talon(1)), ::std::unique_ptr<Talon>(new Talon(6)));
+
+ rollers_writer.set_rollers_low_goal_talon(
+ ::std::unique_ptr<Talon>(new Talon(4)));
+ ::std::thread rollers_writer_thread(::std::ref(rollers_writer));
+
+ ::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
+ new ::frc971::wpilib::BufferedPcm());
+ SolenoidWriter solenoid_writer(pcm);
+ solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(6));
+ solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(5));
+ solenoid_writer.set_rollers_front(pcm->MakeSolenoid(2));
+ solenoid_writer.set_rollers_back(pcm->MakeSolenoid(4));
+
+ // Don't change the following IDs.
+ solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(9));
+ solenoid_writer.set_compressor_relay(make_unique<Relay>(0));
+ ::std::thread solenoid_thread(::std::ref(solenoid_writer));
+
+ // Wait forever. Not much else to do...
+ PCHECK(select(0, nullptr, nullptr, nullptr, nullptr));
+
+ LOG(ERROR, "Exiting WPILibRobot\n");
+
+ joystick_sender.Quit();
+ joystick_thread.join();
+ reader.Quit();
+ reader_thread.join();
+ gyro_sender.Quit();
+ gyro_thread.join();
+
+ drivetrain_writer.Quit();
+ drivetrain_writer_thread.join();
+
+ rollers_writer.Quit();
+ rollers_writer_thread.join();
+
+ solenoid_writer.Quit();
+ solenoid_thread.join();
+
+ ::aos::Cleanup();
+ }
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+START_ROBOT_CLASS(::frc971::wpilib::WPILibRobot);